// =====================================================================
// S100 Computers ESP32 SD Card Firmware
// =====================================================================
//
// This is the ESP32 Runtime program for the S100 Computers 2CF+1SD & 2SD boards.
//
// You MUST adjust the first set of #define directives for the desired target board!!!
//
// Program assumes an ESP32 S3 DevKitC1 or a Waveshare ESP32-S3 board.
//
// This version was produced by Wayne Warthen (wwarthen@gmail.com) and
// Michael Petry and is a derivative of the work by John Monahan.
//
// http://www.s100computers.com/My%20System%20Pages/IDE%202CF+SD%20Board/IDE%202CF+1SD%20Board.htm
// http://www.s100computers.com/My%20System%20Pages/Dual%20SD%20card%20Board/Dual%20SD%20card%20Board.htm
//
// Compile for board definition "ESP32S3 Dev Module"
//
// Libraries required:
//  - SSD1306Ascii by Bill Greiman
//  - SdFat - Adafruit Fork by Bill Greiman
//
// Notes:
//   - SD.begin() will fail if the media has no valid FAT filesystem!
//     Note: SD.begin() has been superseded by SdFs.cardBegin() to avoid this.
//   - Seek is constrained to 16 bits limiting access to first 32MB of media
//     A new API function supporting 32 bits has been added to address this.
//   - On 2SD, GPIO 48 is used for SD2 MOSI which is also used for the
//     built-in LED on the Dev Kit board.  Does not seem to be causing a
//     problem, but the built-in LED follows the state of MOSI.
//   - GPIO pins 19 & 20 are connected to the USB data lines on the
//     Dev Kit board.  They are also used by the S100 boards for
//     reading the diagnostic switch and for signaling data ready.
//     The ESP32-S3 doc refers to a way to disable the USB functionality,
//     but that is not yet implemented here.
//   - A race condition exists during ESP32 -> S100 transfers
//     requiring an arbitrary delay prior to writing output data.
//     See https://groups.google.com/g/s100computers/c/ekfh5jTb7NM
//   - The ESP32 is not reset when the board is reset.  The dedicated
//     ESP32 reset button must be used if you need to reset the ESP32
//     on-the-fly, otherwise it will be reset only at power cycle.
//
// Updates:
//   - For 2CF1SD board, return error for INIT_DRIVE_2 and SEL_DRIVE_2
//   - Added check for SD.begin() failure
//   - Switched from SD to SdFs
//   - Use cardBegin() instead of begin() to avoid FAT filesystem requirement
//   - Support SD Card Type 1
//   - Add 1 microsecond delay prior to overwriting output data pins to avoid
//     race condition.
//   - Made the above delay adjustable via the WRITE_DELAY #define just
//     below these comments.  A delay of 1-3 microseconds should work.  The
//     slower your CPU, the more delay required.
//   - Integrated direct register data output from Michael Petry.
//   - Implement buffered/timed OLED display updates to reduce
//     performance impact.
//   - Modified FORMAT command to start formatting from the currently
//     selected sector.
//   - Wrapped GPIO pin pulsing in a critical section to avoid having
//     the pulse affected by multi-tasking/interrupts.
//   - Moved OLED display updates to a background task with proper
//     task synchronization stuff.  Seems to avoid performance impact
//     while allowing pretty fast display refresh rate.
//   - Double buffer display updates to eliminate flicker.
//
// ToDo:
//   - Must check device initialized at start of READ/WRITE/FORMAT!!! (done)
//   - Function to return firmware version to host (done)
//   - Function to return Card Type to host (done)
//   - Function to set 32-bit LBA (done)
//   - Function to return media capacity to host (done)
//   - Function to return card info to host (CID, CSD) (done)
//   - Function to write to OLED display (done)
//   - Function to echo a string (done)
//   - Move OLED screen updates to a background thread? (done)
//   - Add a #define to control serial debug output? (done)
//
//
//                                     ESP32-S3
//     2SD      2CF+1SD    Func        DevKitC1        Func    2CF+1SD       2SD
//  ---------  ---------  ------     +----------+     ------  ---------   ---------
//                           GND --- | 22    22 | --- GND
//                           GND --- | 21    21 | <-- 5V0
//  DIAG       DIAG       GPIO19 --> | 20    20 | --> GPIO14  SD1_SCK     SD2_SCK
//  RCVACT     RCVACT     GPIO20 --> | 19    19 | --> GPIO13  SD1_MOSI    SD1_MOSI
//  SENDACT    SENDACT    GPIO21 --> | 18    18 | <-- GPIO12  SD1_MISO    SD1_MISO
//  SD2_CS     STAT6      GPIO47 <-- | 17    17 | --> GPIO11  SD1_CS      SD1_CS
//  SD2_MOSI   N/C        GPIO48 <-- | 16    16 | --> GPIO10  ERRLED      ERRLED
//  SD2_MISO   N/C        GPIO45 --> | 15    15 | --> GPIO9   I2CSCL      I2CSCL
//  BOOT       BOOT        GPIO0 --> | 14    14 | --- GPIO46  N/C         N/C
//  DOUT0      DOUT0      GPIO35 <-- | 13    13 | --> GPIO3   STAT3       SD1_SCK
//  DOUT1      DOUT1      GPIO36 <-- | 12    12 | <-> GPIO8   I2CSDA      I2CSDA
//  DOUT2      DOUT2      GPIO37 <-- | 11    11 | <-- GPIO18  DIN7        DIN7
//  DOUT3      DOUT3      GPIO38 <-- | 10    10 | <-- GPIO17  DIN6        DIN6
//  DOUT4      DOUT4      GPIO39 <-- | 9      9 | <-- GPIO16  DIN5        DIN5
//  DOUT5      DOUT5      GPIO40 <-- | 8      8 | <-- GPIO15  DIN4        DIN4
//  DOUT6      DOUT6      GPIO41 <-- | 7      7 | <-- GPIO7   DIN3        DIN3
//  DOUT7      DOUT7      GPIO42 <-- | 6      6 | <-- GPIO6   DIN2        DIN2
//  RCV        RCV         GPIO2 <-- | 5      5 | <-- GPIO5   DIN1        DIN1
//  SEND       SEND        GPIO1 <-- | 4      4 | <-- GPIO4   DIN0        DIN0
//                         U0RXD --> | 3      3 | <-- RST
//                         U0TXD <-- | 2      2 | --> 3V3
//                           GND --- | 1      1 | --> 3V3
//                                   +----------+
//
// =====================================================================
// User Configuration Settings
// =====================================================================
//
// Define EXACTLY 1 of the following boards:
//#define BOARD_2CF1SD
#define BOARD_2SD
//
#define WRITE_DELAY 3  // Delay in microseconds prior to data writes (see comments above)
//
#define DIRECTGPIO_INPUT   // Use direct GPIO for data input (faster)
#define DIRECTGPIO_OUTPUT  // Use direct GPIO for data output (faster)
//
#define DIAG_LEVEL DIAG_WARNING  // DIAG_[NONE|ERROR|WARNING|INFO|VERBOSE]
//
// =====================================================================

#define FWVER_MAJOR 1
#define FWVER_MINOR 5
#define FWVER_DATE "7-Sep-2025"

// Board identifiers
#define BOARD_ID_2CF1SD 1
#define BOARD_ID_2SD 2

// Diagnostic levels
#define DIAG_NONE 0
#define DIAG_ERROR 4
#define DIAG_WARNING 8
#define DIAG_INFO 12
#define DIAG_VERBOSE 16

// Miscellaneous GPIO assignments
#define GPIO_ERRLED GPIO_NUM_10  // Error Status LED (active LOW)
#define GPIO_DIAG GPIO_NUM_19    // Diagnostic display activation switch

// Control signal GPIO assignments
#define GPIO_SEND GPIO_NUM_1      // Data write strobe (pulse HIGH)
#define GPIO_RCV GPIO_NUM_2       // Data read acknowledge (pulse HIGH)
#define GPIO_SENDACT GPIO_NUM_21  // Send data status (HIGH if unread output data)
#define GPIO_RCVACT GPIO_NUM_20   // Receive data status (HIGH if unread input data)

// 8-bit output port GPIO assignments
#define GPIO_DOUT0 GPIO_NUM_35  // LSB
#define GPIO_DOUT1 GPIO_NUM_36
#define GPIO_DOUT2 GPIO_NUM_37
#define GPIO_DOUT3 GPIO_NUM_38
#define GPIO_DOUT4 GPIO_NUM_39
#define GPIO_DOUT5 GPIO_NUM_40
#define GPIO_DOUT6 GPIO_NUM_41
#define GPIO_DOUT7 GPIO_NUM_42  // MSB

// 8-bit input port GPIO assignments
#define GPIO_DIN0 GPIO_NUM_4  // LSB
#define GPIO_DIN1 GPIO_NUM_5
#define GPIO_DIN2 GPIO_NUM_6
#define GPIO_DIN3 GPIO_NUM_7
#define GPIO_DIN4 GPIO_NUM_15
#define GPIO_DIN5 GPIO_NUM_16
#define GPIO_DIN6 GPIO_NUM_17
#define GPIO_DIN7 GPIO_NUM_18  // MSB

// SD Card GPIO assignments for 2CF+1SD Board
#ifdef BOARD_2CF1SD

#define BOARD_ID BOARD_ID_2CF1SD

#define SD1_CS GPIO_NUM_11
#define SD1_SCK GPIO_NUM_14
#define SD1_MOSI GPIO_NUM_13
#define SD1_MISO GPIO_NUM_12

#define SD2_CS GPIO_NUM_NC
#define SD2_SCK GPIO_NUM_NC
#define SD2_MOSI GPIO_NUM_NC
#define SD2_MISO GPIO_NUM_NC

#endif

// SD Card GPIO assignments for 2SD Board
#ifdef BOARD_2SD

#define BOARD_ID BOARD_ID_2SD

#define SD1_CS GPIO_NUM_11
#define SD1_SCK GPIO_NUM_3
#define SD1_MOSI GPIO_NUM_13
#define SD1_MISO GPIO_NUM_12

#define SD2_CS GPIO_NUM_47
#define SD2_SCK GPIO_NUM_14
#define SD2_MOSI GPIO_NUM_48
#define SD2_MISO GPIO_NUM_45

#endif

#include "soc/gpio_struct.h"
#include "SdFat.h"

#include "SSD1306AsciiWire.h"
// #include "fonts/TimesNewRoman16.h"
#include "fonts/X11fixed7x14B.h"

#define SSD1306

#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels

//#define gpioSet digitalWrite
//#define gpioGet digitalRead
#define gpioSet gpio_set_level
#define gpioGet gpio_get_level

SSD1306AsciiWire ssd;

// 1306 OLED Display
#define I2C_ADDRESS 0x3C
#define I2CSDA GPIO_NUM_8
#define I2CSCL GPIO_NUM_9

// SPI configuration (2 channels)
#define SD1_CONFIG SdSpiConfig(SD1_CS, SHARED_SPI, SD_SCK_MHZ(16), &hspi)
#define SD2_CONFIG SdSpiConfig(SD2_CS, SHARED_SPI, SD_SCK_MHZ(16), &fspi)

// Command processing states
#define STATE_IDLE 0
#define STATE_CMD 1

SPIClass hspi = SPIClass(HSPI);
SPIClass fspi = SPIClass(FSPI);

SdSpiConfig sdConfig[2] = { SD1_CONFIG, SD2_CONFIG };

SdFs SD[2];

uint8_t bufData[1024];
char curDrive = '?';
int curSD = 0;
uint32_t sector = 0;
int state = STATE_IDLE;
volatile bool ssdEnab = false;
char ssdBuf[256];
TaskHandle_t ssdTask = NULL;
SemaphoreHandle_t ssdBufMutex = NULL;

#ifdef BOARD_2CF1SD
const char* boardName = "2CF+1SD";
#endif

#ifdef BOARD_2SD
const char* boardName = "Dual SD";
#endif

// ESP32 Command definitions:
#define CMD_INIT_DRIVE_1 0x80   // Initialize SD drive C:
#define CMD_INIT_DRIVE_2 0x81   // Initialize SD drive D: (not used here)
#define CMD_SEL_DRIVE_1 0x82    // (Re)select an already initialized drive C:
#define CMD_SEL_DRIVE_2 0x83    // (Re)select an already initialized drive D: (not used here)
#define CMD_SET_TRK_SEC 0x84    // Set new current TRACK+SECTOR on current drive (new)
#define CMD_READ_SECTOR 0x85    // Read data from the CURRENT sector (on current track,drive)
#define CMD_WRITE_SECTOR 0x86   // Write data to the CURRENT sector (on current track,drive)
#define CMD_FORMAT_SECTOR 0x87  // Format the CURRENT sector with 0xE5's
#define CMD_RESET_ESP32 0x88    // Reset the ESP32 CPU

#define CMD_INIT_CF_DRIVE_A 0x89  // Initialize CF drive A: (OLED display info only)
#define CMD_INIT_CF_DRIVE_B 0x8A  // Initialize CF drive B: (OLED display info only)
#define CMD_READ_CF_SECTOR 0x8B   // Read data from the CURRENT CF card sector (on current track,drive) (OLED display info only)
#define CMD_WRITE_CF_SECTOR 0x8C  // Write data to the CURRENT CF card sector (on current track,drive) (OLED display info only)

#define CMD_START_ESP32 0x90  // Handshake on startup with the S100 bus (deprecated)
#define CMD_S100_ACK 0x91     // Handshake returned to board from S100 bus (deprecated)

#define CMD_FWVER 0x90   // Report ESP32 firmware version
#define CMD_SETLBA 0x91  // Set new current LBA (32-bit)
#define CMD_TYPE 0x92    // Report Card Type of selected SD Card
#define CMD_CAP 0x93     // Report Capacity (sectors) of selected SD Card
#define CMD_CID 0x94     // Report CID data of selected SD Card
#define CMD_CSD 0x95     // Report CSD data of selected SD Card
#define CMD_DISP 0x96    // Write string to OLED display
#define CMD_ECHO 0x97    // Echo received string back to host

#define CMD_FIRST CMD_INIT_DRIVE_1
#define CMD_LAST CMD_ECHO

/*
 *  ->CMD_INIT_DRIVE_1, <-STATUS
 *  ->CMD_INIT_DRIVE_2, <-STATUS
 *  ->CMD_SEL_DRIVE_1, <-STATUS
 *  ->CMD_SEL_DRIVE_2, <-STATUS
 *  ->CMD_SET_TRK_SEC, ->[Track (byte), Sector (byte)], <-STATUS
 *  ->CMD_READ_SECTOR, <-[Sector Data (512 bytes)], <-STATUS
 *  ->CMD_WRITE_SECTOR, ->[Sector Data (512 bytes)], <-STATUS
 *  ->CMD_FORMAT_SECTOR, <-STATUS
 *  ->CMD_RESET_ESP32
 *
 *  ->CMD_INIT_CF_DRIVE_A, <-STATUS
 *  ->CMD_INIT_CF_DRIVE_B, <-STATUS
 *  ->CMD_READ_CF_SECTOR, <-STATUS
 *  ->CMD_WRITE_CF_SECTOR, <-STATUS
 *  ->CMD_START_ESP32, <-STATUS (not implemented)
 *  
 *  Added in v1.4:
 *  ->CMD_FWVER, <-[BoardID (byte), Ver-Major (byte), Ver-Minor (byte)], <-STATUS
 *  ->CMD_SETLBA, ->[LBA value (4 bytes, MS first)], <-STATUS
 *  ->CMD_TYPE, <-[SD Card Type (1 byte)], <-STATUS
 *  ->CMD_CAP, <-[Sector Count (4 bytes, MS first)], <-STATUS
 *  ->CMD_CID, <-[CID Data (16 bytes), <-STATUS
 *  ->CMD_CSD, <-[CSD Data (16 bytes), <-STATUS
 *  ->CMD_DISP, ->[null terminated string], <-STATUS
 *  ->CMD_ECHO, ->[length (msb,lsb)], ->[data], <-[data], <-STATUS
 */

// Command result (STATUS) values
#define STAT_OK 0x00   // Operation completed successfully
#define STAT_ERR 0x1A  // Operation failed

void inline pulseHigh(gpio_num_t gpioNum) {
  static portMUX_TYPE _spinlock = portMUX_INITIALIZER_UNLOCKED;

  taskENTER_CRITICAL(&_spinlock);
  gpioSet(gpioNum, HIGH);  // pulse U15 LE high
  gpioSet(gpioNum, LOW);   // return U15 LE low
  taskEXIT_CRITICAL(&_spinlock);
}

void inline pulseLow(gpio_num_t gpioNum) {
  static portMUX_TYPE _spinlock = portMUX_INITIALIZER_UNLOCKED;

  taskENTER_CRITICAL(&_spinlock);
  gpioSet(gpioNum, LOW);   // return U15 LE low
  gpioSet(gpioNum, HIGH);  // pulse U15 LE high
  taskEXIT_CRITICAL(&_spinlock);
}

void diagPrintf(int diagLvl, const char* fmt, ...) {
  va_list args;
  char buf[256];

  if (diagLvl > DIAG_LEVEL) return;

  va_start(args, fmt);
  vsprintf(buf, fmt, args);
  Serial.print(buf);
  va_end(args);
}

void ssdPrintf(bool flag, const char* fmt, ...) {
  va_list args;

  if (!flag) return;

  va_start(args, fmt);

  xSemaphoreTake(ssdBufMutex, portMAX_DELAY);  // enter critical section
  vsprintf(ssdBuf + strlen(ssdBuf), fmt, args);
  xSemaphoreGive(ssdBufMutex);  // exit critical section

  xTaskNotifyGive(ssdTask);

  va_end(args);
}

void ssdClrPrintf(bool flag, const char* fmt, ...) {
  va_list args;

  if (!flag) return;

  va_start(args, fmt);

  xSemaphoreTake(ssdBufMutex, portMAX_DELAY);  // enter critical section
  vsprintf(ssdBuf, fmt, args);
  xSemaphoreGive(ssdBufMutex);  // exit critical section

  xTaskNotifyGive(ssdTask);

  va_end(args);
}

void ssdPaintOld(void) {
  char buf[sizeof(ssdBuf)];

  xSemaphoreTake(ssdBufMutex, portMAX_DELAY);
  strcpy(buf, ssdBuf);
  xSemaphoreGive(ssdBufMutex);

  ssd.clear();
  ssd.home();
  ssd.print(buf);
}

void ssdPaint(void) {
  char buf[sizeof(ssdBuf)];

  xSemaphoreTake(ssdBufMutex, portMAX_DELAY);

  // Copy to local buf while converting all \n to \0
  char* ptrSrc;
  char* ptrDest;
  for (ptrSrc = ssdBuf, ptrDest = buf; *ptrSrc != '\0'; ptrSrc++) {
    if (*ptrSrc == '\n')
      *ptrDest++ = '\0';
    else
      *ptrDest++ = *ptrSrc;
  }
  memset(ptrDest, '\0', 5);

  xSemaphoreGive(ssdBufMutex);

  // Print 4 lines to display clearing to EOL
  char* ptr = buf;
  ssd.home();
  for (int i = 0; i < 4; i++) {
    ssd.print(ptr);
    ssd.clearToEOL();
    ssd.print('\n');
    ptr += strlen(ptr) + 1;
  }
}

void sdErrorPrint(SdFs* sd) {
  if (sd->sdErrorCode()) {
    diagPrintf(DIAG_ERROR, "\r\nSD Error Code = 0x%0.2X, ", int(sd->sdErrorCode()));
    if (DIAG_LEVEL >= DIAG_ERROR) printSdErrorText(&Serial, sd->sdErrorCode());
    diagPrintf(DIAG_ERROR, "\r\nSD Error Data = 0x%0.2X", int(sd->sdErrorData()));
  }
}

// Setup the PARALLEL port 8 bits to output mode
void initDataOutput() {
  pinMode(GPIO_DOUT7, OUTPUT);
  pinMode(GPIO_DOUT6, OUTPUT);
  pinMode(GPIO_DOUT5, OUTPUT);
  pinMode(GPIO_DOUT4, OUTPUT);
  pinMode(GPIO_DOUT3, OUTPUT);
  pinMode(GPIO_DOUT2, OUTPUT);
  pinMode(GPIO_DOUT1, OUTPUT);
  pinMode(GPIO_DOUT0, OUTPUT);
}

// Setup the PARALLEL port 8 bits to input mode
void initDataInput() {
  pinMode(GPIO_DIN7, INPUT);
  pinMode(GPIO_DIN6, INPUT);
  pinMode(GPIO_DIN5, INPUT);
  pinMode(GPIO_DIN4, INPUT);
  pinMode(GPIO_DIN3, INPUT);
  pinMode(GPIO_DIN2, INPUT);
  pinMode(GPIO_DIN1, INPUT);
  pinMode(GPIO_DIN0, INPUT);
}

void writeData(uint8_t data) {

#if (WRITE_DELAY > 0)
  delayMicroseconds(WRITE_DELAY);  // See comments at start
#endif

#ifdef DIRECTGPIO_OUTPUT
  // The data bits (0-7) map directly to GPIO register bits (3-10).
  // We can create the final mask by simply shifting the 8-bit data
  // value left by 3 positions.
  uint32_t set_mask = (uint32_t)data << 3;

  // To find the bits to clear, we invert the data, mask it to 8 bits,
  // and then shift it to the correct position.
  uint32_t clear_mask = (uint32_t)(~data & 0xFF) << 3;

  // Write to the high-numbered GPIO registers.
  // W1TS = Write 1 to Set: Sets pins HIGH without affecting others.
  // W1TC = Write 1 to Clear: Sets pins LOW without affecting others.
  GPIO.out1_w1ts.val = set_mask;
  GPIO.out1_w1tc.val = clear_mask;
#else
  gpioSet(GPIO_DOUT7, (data & 0x80) >> 7);
  gpioSet(GPIO_DOUT6, (data & 0x40) >> 6);
  gpioSet(GPIO_DOUT5, (data & 0x20) >> 5);
  gpioSet(GPIO_DOUT4, (data & 0x10) >> 4);
  gpioSet(GPIO_DOUT3, (data & 0x08) >> 3);
  gpioSet(GPIO_DOUT2, (data & 0x04) >> 2);
  gpioSet(GPIO_DOUT1, (data & 0x02) >> 1);
  gpioSet(GPIO_DOUT0, (data & 0x01));
#endif

  pulseHigh(GPIO_SEND);
}

char readData() {
  char value;

#ifdef DIRECTGPIO_INPUT
  uint32_t raw = GPIO.in;

  // Extract GPIO4–7 → bits 0..3
  uint8_t low = (raw >> 4) & 0x0F;

  // Extract GPIO15–18 → already shift into bits 4..7
  uint8_t high = (raw >> 11) & 0xF0;

  // Combine directly
  value = high | low;

#else

  value = gpioGet(GPIO_DIN0);
  value += gpioGet(GPIO_DIN1) << 1;
  value += gpioGet(GPIO_DIN2) << 2;
  value += gpioGet(GPIO_DIN3) << 3;
  value += gpioGet(GPIO_DIN4) << 4;
  value += gpioGet(GPIO_DIN5) << 5;
  value += gpioGet(GPIO_DIN6) << 6;
  value += gpioGet(GPIO_DIN7) << 7;

#endif

  pulseHigh(GPIO_RCV);

  return (value);
}

//Get an S100 bus Command or byte
char GetData() {
  while (gpioGet(GPIO_RCVACT) == LOW)  // wait for data
    ;
  return readData();  // get byte
}

// Get a 16 bit value from S100 bus
uint32_t getTrkSec() {
  uint32_t lba;

  lba = 0;
  lba += GetData() << 8;  // Track
  lba += GetData();       // Sector

  return lba;
}

uint32_t getLBA() {
  uint32_t lba;

  lba = 0;
  lba += GetData() << 24;
  lba += GetData() << 16;
  lba += GetData() << 8;
  lba += GetData();

  return lba;
}

// Get a 16 bit value from S100 bus
uint16_t GetDataWord() {
  uint16_t msb, lsb;

  lsb = GetData();  // LSB
  msb = GetData();  // MSB

  return (word(msb, lsb));
}

// Send an S100 bus Command
void SendData(char data) {
  while (gpioGet(GPIO_SENDACT) == HIGH)
    ;
  writeData(data);
}

void SetErrorLED() {
  gpioSet(GPIO_ERRLED, LOW);
}

void ClearErrorLED() {
  gpioSet(GPIO_ERRLED, HIGH);
}

int sdInit(int sdNum) {
  uint8_t cardType;

  curSD = sdNum;
  curDrive = 'C' + sdNum;

  diagPrintf(DIAG_INFO, "\r\nDrive %c: Initializing...", curDrive);

  if (!SD[curSD].cardBegin(sdConfig[curSD])) {
    SetErrorLED();
    diagPrintf(DIAG_ERROR, "\r\nDrive %c: Initialization Failed", curDrive);
    sdErrorPrint(&SD[curSD]);
    ssdClrPrintf(ssdEnab, "Drive %c:\nInit Failed", curDrive);
    return STAT_ERR;
  }

  cardType = SD[curSD].card()->type();
  diagPrintf(DIAG_INFO, "\r\nDrive %c: Initialization Complete, Card Type %i", curDrive, cardType);

  ssdClrPrintf(ssdEnab, "Drive %c:\nInitialized\nCard Type %i", curDrive, cardType);

  ClearErrorLED();
  return STAT_OK;
}

void ssdProc(void* pvParameters) {
  diagPrintf(DIAG_INFO, "\r\nTask ssdProc running...");
  for (;;) {
    ssdEnab = (gpioGet(GPIO_DIAG) == HIGH);
    if (int n = ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100))) {
      (void)n;
      //diagPrintf(DIAG_NONE, "\r\nx=%i", n);
      ssdPaint();
    }
  }
  diagPrintf(DIAG_INFO, "\r\nTask ssdProc exit.");
}

void setup() {
  Serial.begin(115200);
  diagPrintf(DIAG_NONE, "\r\nS100 %s, Firmware v%i.%i, %s", boardName, FWVER_MAJOR, FWVER_MINOR, FWVER_DATE);

  diagPrintf(DIAG_INFO, "\r\nStarting Setup...");

  pinMode(GPIO_DIAG, INPUT);  // Switch for diagnostic OLED display

  pinMode(GPIO_ERRLED, OUTPUT);  // Error LED
  gpioSet(GPIO_ERRLED, HIGH);    // Clear error LED

  pinMode(GPIO_SEND, OUTPUT);    // Data write strobe (pulse HIGH)
  pinMode(GPIO_RCV, OUTPUT);     // Data read acknowledge (pulse HIGH)
  pinMode(GPIO_SENDACT, INPUT);  // Send data status (HIGH if unread output data)
  pinMode(GPIO_RCVACT, INPUT);   // Receive data status (HIGH if unread input data)

  initDataInput();
  initDataOutput();

#ifdef BOARD_2CF1SD
  pinMode(GPIO_NUM_47, OUTPUT);
  gpioSet(GPIO_NUM_47, LOW);
  pinMode(GPIO_NUM_3, OUTPUT);
  gpioSet(GPIO_NUM_3, LOW);
#endif

  Wire.begin(I2CSDA, I2CSCL);  // enable & set I2C pins
  Wire.setClock(400000L);

  ssd.begin(&Adafruit128x64, I2C_ADDRESS);
  // ssd.setFont(Adafruit5x7);
  // ssd.setFont(TimesNewRoman16);
  ssd.setFont(X11fixed7x14B);
  // ssd.set2X();

  ssdBuf[0] = '\0';

  if (xTaskCreate(ssdProc, "ssdProc", 10000, NULL, tskIDLE_PRIORITY + 1, &ssdTask) == pdPASS)
    diagPrintf(DIAG_INFO, "\r\nDisplay processing task created");
  else
    diagPrintf(DIAG_ERROR, "\r\nDisplay processing task creation failed");
  delay(5);

  if ((ssdBufMutex = xSemaphoreCreateMutex()))
    diagPrintf(DIAG_INFO, "\r\nDisplay buffer mutex created");
  else
    diagPrintf(DIAG_ERROR, "\r\nDisplay buffer mutex creation failed");

  ssdClrPrintf(true, "S100 %s\nFirmware v%i.%i\n%s\nBoard Ready!", boardName, FWVER_MAJOR, FWVER_MINOR, FWVER_DATE);

  if (!hspi.begin(SD1_SCK, SD1_MISO, SD1_MOSI, SD1_CS))
    diagPrintf(DIAG_ERROR, "\r\nHSPI begin() failed!");
#ifdef BOARD_2SD
  if (!fspi.begin(SD2_SCK, SD2_MISO, SD2_MOSI, SD2_CS))
    diagPrintf(DIAG_ERROR, "\r\nFSPI begin() failed!");
#endif

  sdInit(0);
#ifdef BOARD_2SD
  sdInit(1);
#endif

  sector = 0;  // initialize to Track 0, sector 0

  pulseHigh(GPIO_RCV);  // Clear incoming

  gpioSet(GPIO_SEND, LOW);  // Initialize LOW

  ClearErrorLED();

  state = STATE_IDLE;

  diagPrintf(DIAG_INFO, "\r\nSetup Complete");
}

int runCmd(uint8_t cmd) {
  diagPrintf(DIAG_VERBOSE, "\r\nProcessing Command 0x%0.2X", cmd);

  switch (cmd) {
    case CMD_INIT_DRIVE_1:  // 0x80, Initialize SD card 1
      return sdInit(0);

    case CMD_INIT_DRIVE_2:  // 0x81, Initialize SD card 2
#ifdef BOARD_2SD
      return sdInit(1);
#else
      diagPrintf(DIAG_WARNING, "\r\nDrive D: Initialize, Not Present");
      ssdClrPrintf(ssdEnab, "Drive D:\nInitialize\nNot Present");
      return STAT_ERR;
#endif

    case CMD_SEL_DRIVE_1:  // 0x82, Reselect SD card 1
      curSD = 0;
      curDrive = 'C';
      diagPrintf(DIAG_VERBOSE, "\r\nDrive %c: Selected", curDrive);
      //ssdClrPrintf(ssdEnab, "Drive %c: Selected", curDrive);
      return STAT_OK;

    case CMD_SEL_DRIVE_2:  // 0x83, Reselect SD card 2
#ifdef BOARD_2SD
      curSD = 1;
      curDrive = 'D';
      diagPrintf(DIAG_VERBOSE, "\r\nDrive %c: Selected", curDrive);
      //ssdClrPrintf(ssdEnab, "Drive %c: Selected", curDrive);
      return STAT_OK;
#else
      diagPrintf(DIAG_WARNING, "\r\nDrive D: Select, Not Present!");
      //ssdClrPrintf(ssdEnab, "Drive D:\nSelect Error\nNot Present");
      return STAT_ERR;
#endif

    case CMD_INIT_CF_DRIVE_A:  // 0x89, Initialize CF Card 1 (OLED Display Update Only)
      curDrive = 'A';
      diagPrintf(DIAG_INFO, "\r\nDrive %c: Initialization Complete", curDrive);
      ssdClrPrintf(ssdEnab, "Drive %c:\nInitialized", curDrive);
      return STAT_OK;

    case CMD_INIT_CF_DRIVE_B:  // 0x8A, Initialize CF Card 2 (OLED Display Update Only)
      curDrive = 'B';
      diagPrintf(DIAG_INFO, "\r\nDrive %c: Initialization Complete", curDrive);
      ssdClrPrintf(ssdEnab, "Drive %c:\nInitialized", curDrive);
      return STAT_OK;

    case CMD_SET_TRK_SEC:  // 0x84, Set block address for subsequent read/write
      sector = getTrkSec();
      diagPrintf(DIAG_VERBOSE, "\r\nSet Sector = 0x%0.8X", sector);
      //ssdClrPrintf(ssdEnab, "Sector 0x%0.8X", sector);
      return STAT_OK;

    case CMD_READ_SECTOR:  // 0x85, Read an SD Card sector
      if (!SD[curSD].card() || !SD[curSD].card()->readSector(sector, bufData)) {
        for (int i = 0; i < 512; i++)
          SendData(0x00);
        diagPrintf(DIAG_ERROR, "\r\nDrive %c: Read Error @ Sector 0x%0.8X", curDrive, sector);
        ssdClrPrintf(ssdEnab, "Drive %c:\nRead Error\nSector 0x%0.8X", curDrive, sector);
        SetErrorLED();
        return STAT_ERR;
      } else {
        for (int i = 0; i < 512; i++)  // No Errors
          SendData(bufData[i]);
        diagPrintf(DIAG_INFO, "\r\nDrive %c: Read Sector 0x%0.8X", curDrive, sector);
        ssdClrPrintf(ssdEnab, "Drive %c: Read\nSector 0x%0.8X", curDrive, sector);
        return STAT_OK;
      }

    case CMD_READ_CF_SECTOR:  // 0x8B, Read a CF Card sector (OLED display update only)
      diagPrintf(DIAG_INFO, "\r\nDrive %c: Read Sector 0x%0.8X", curDrive, sector);
      ssdClrPrintf(ssdEnab, "Drive %c: Read\nSector 0x%0.8X", curDrive, sector);
      return STAT_OK;

    case CMD_WRITE_SECTOR:  // 0x86, Write an SD Card sector
      for (int i = 0; i < 512; i++)
        bufData[i] = GetData();  // get sector data to buffer
      if (!SD[curSD].card() || !SD[curSD].card()->writeSector(sector, bufData)) {
        diagPrintf(DIAG_ERROR, "\r\nDrive %c: Write Error @ Sector 0x%0.8X", curDrive, sector);
        ssdClrPrintf(ssdEnab, "Drive %c:\nWrite Error\nSector 0x%0.8X", curDrive, sector);
        SetErrorLED();
        return STAT_ERR;
      } else {
        diagPrintf(DIAG_INFO, "\r\nDrive %c: Write Sector 0x%0.8X", curDrive, sector);
        ssdClrPrintf(ssdEnab, "Drive %c: Write\nSector 0x%0.8X", curDrive, sector);
        return STAT_OK;
      }

    case CMD_WRITE_CF_SECTOR:  // 0x8C, Write a CF Card sector (OLED display update only)
      diagPrintf(DIAG_INFO, "\r\nDrive %c: Write Sector 0x%0.8X", curDrive, sector);
      ssdClrPrintf(ssdEnab, "Drive %c: Write\nSector 0x%0.8X", curDrive, sector);
      return STAT_OK;

    case CMD_FORMAT_SECTOR:  // 0x87, Format an SD Card sector
      {
        uint32_t sector_count = GetDataWord();  // Get 16 bit sector count
        while (sector_count > 0) {
          diagPrintf(DIAG_INFO, "\r\nDrive %c: Format Sector 0x%0.8X", curDrive, sector);
          ssdClrPrintf(ssdEnab, "Drive %c:Format\nSector 0x%0.8X", curDrive, sector);

          memset(bufData, 0xE5, 512);  // Prepare sector data buffer (fill with 0xE5)

          if (sector != 0) {  // Do not format sector 0
            if (!(SD[curSD].card()) || !SD[curSD].card()->writeSector(sector, bufData)) {
              diagPrintf(DIAG_ERROR, "\r\nDrive %c: Format Error @ Sector 0x%0.8X", curDrive, sector);
              ssdClrPrintf(ssdEnab, "Drive %c:\nFormat Error\nSector 0x%0.8X", curDrive, sector);
              SetErrorLED();
              return STAT_ERR;
            }
          }
          --sector_count;
          ++sector;
        }
        return STAT_OK;
      }

    case CMD_RESET_ESP32:  // 0x88, Reset the ESP32
      ClearErrorLED();
      ssdClrPrintf(true, "Rebooting...");
      delay(1000);
      ESP.restart();
      break;

    case CMD_FWVER:  // 0x90, Report ESP32 firmware version
      SendData(BOARD_ID);
      SendData(FWVER_MAJOR);
      SendData(FWVER_MINOR);
      return STAT_OK;

    case CMD_SETLBA:  // 0x91, Set new current LBA (32-bit)
      sector = getLBA();
      diagPrintf(DIAG_VERBOSE, "\r\nSet Sector = 0x%0.8X", sector);
      ssdClrPrintf(false, "Sector 0x%0.8X", sector);
      return STAT_OK;

    case CMD_TYPE:  // 0x92, Report Card Type of selected SD Card
      if (!SD[curSD].card()) {
        SendData(0x00);
        return STAT_ERR;
      }
      SendData(SD[curSD].card()->type());
      return STAT_OK;

    case CMD_CAP:  // 0x93, Report Capacity (sectors) of selected SD Card
      {
        csd_t csd;
        uint32_t cap;
        if (!SD[curSD].card() || !SD[curSD].card()->readCSD(&csd)) {
          for (int i = 0; i < 4; i++)
            SendData(0x00);
          return STAT_ERR;
        }
        cap = csd.capacity();
        SendData((cap >> 24) & 0xFF);
        SendData((cap >> 16) & 0xFF);
        SendData((cap >> 8) & 0xFF);
        SendData(cap & 0xFF);
        return STAT_OK;
      }

    case CMD_CID:  // 0x94, Report CID data of selected SD Card
      {
        cid_t cid;
        uint8_t* u8 = reinterpret_cast<uint8_t*>(&cid);
        if (!SD[curSD].card() || !SD[curSD].card()->readCID(&cid)) {
          for (int i = 0; i < 16; i++)
            SendData(0x00);
          return STAT_ERR;
        }
        for (int i = 0; i < 16; i++)
          SendData(u8[i]);
        return STAT_OK;
      }

    case CMD_CSD:  // 0x95, Report CSD data of selected SD Card
      {
        csd_t csd;
        if (!SD[curSD].card() || !SD[curSD].card()->readCSD(&csd)) {
          for (int i = 0; i < 16; i++)
            SendData(0x00);
          return STAT_ERR;
        }
        for (int i = 0; i < 16; i++)
          SendData(csd.csd[i]);
        return STAT_OK;
      }

    case CMD_DISP:  // 0x96, Write received string to OLED Display
      {
        char val;
        int i;
        char buf[sizeof(ssdBuf)];

        i = 0;
        while ((val = GetData()) != 0)
          if (i < (sizeof(ssdBuf) - 1))
            buf[i++] = val;
        buf[i] = '\0';

        ssdClrPrintf(true, buf);
      }
      return STAT_OK;

    case CMD_ECHO:  // 0x97, Echo received string back to host
      {
        int length, i;

        diagPrintf(DIAG_VERBOSE, "\r\nECHO: ");

        ssdClrPrintf(ssdEnab, "*** Echo ***\n*** Echo ***\n*** Echo ***\n");

        length = GetData() << 8;
        length += GetData();

        diagPrintf(DIAG_VERBOSE, "receiving %i bytes", length);

        for (i = 0; i < length; i++) {
          bufData[i] = GetData();
          //diagPrintf(DIAG_VERBOSE, ">%0.2X", bufData[i]);
        }

        diagPrintf(DIAG_VERBOSE, " (done)");

        uint16_t chksum = 0;
        for (i = 0; i < length; i++)
          chksum += bufData[i];

        diagPrintf(DIAG_VERBOSE, ", checksum=0x%0.4X", chksum);

        diagPrintf(DIAG_VERBOSE, ", sending %i bytes", length);

        for (i = 0; i < length; i++) {
          SendData(bufData[i]);
        }

        diagPrintf(DIAG_VERBOSE, " (done)");
      }
      return STAT_OK;
  }

  return STAT_ERR;
}

void loop() {
  if (gpioGet(GPIO_RCVACT) == HIGH) {
    uint8_t byteIn = readData();

    switch (state) {
      case STATE_IDLE:
        if (byteIn == 0x33)
          state = STATE_CMD;
        else
          diagPrintf(DIAG_WARNING, "\r\nUnexpected command prefix byte: 0x%0.2X", byteIn);
        return;

      case STATE_CMD:
        if ((byteIn < CMD_FIRST) || (byteIn > CMD_LAST))
          diagPrintf(DIAG_ERROR, "\r\nInvalid Command 0x%0.2X", byteIn);
        else
          SendData(runCmd(byteIn));
        state = STATE_IDLE;
        return;

      default:
        state = STATE_IDLE;  // in case state becomes invalid
        return;
    }
  }
  return;
}