From bd16b7c69ee27acb5c4527d21650752d0cd95381 Mon Sep 17 00:00:00 2001
From: Maxim Poliakovski <maximumspatium@googlemail.com>
Date: Wed, 21 Aug 2024 21:14:17 +0200
Subject: [PATCH] atahd: implement mandatory SET_MULTIPLE_MODE command.

---
 devices/common/ata/atadefs.h |  1 +
 devices/common/ata/atahd.cpp | 21 ++++++++++++++++++---
 devices/common/ata/atahd.h   |  5 +++--
 3 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/devices/common/ata/atadefs.h b/devices/common/ata/atadefs.h
index af2a55ba1..4377b2142 100644
--- a/devices/common/ata/atadefs.h
+++ b/devices/common/ata/atadefs.h
@@ -137,6 +137,7 @@ enum ATA_Cmd : uint8_t {
     ATAPI_SERVICE                        = 0xA2,
     READ_MULTIPLE                        = 0xC4,
     WRITE_MULTIPLE                       = 0xC5,
+    SET_MULTIPLE_MODE                    = 0xC6,
     READ_DMA                             = 0xC8,
     WRITE_DMA                            = 0xCA,
     STANDBY_IMMEDIATE_E0                 = 0xE0,
diff --git a/devices/common/ata/atahd.cpp b/devices/common/ata/atahd.cpp
index 4f9b1d285..23c179e28 100644
--- a/devices/common/ata/atahd.cpp
+++ b/devices/common/ata/atahd.cpp
@@ -28,6 +28,7 @@ along with this program.  If not, see <https://www.gnu.org/licenses/>.
 #include <machines/machinebase.h>
 #include <memaccess.h>
 
+#include <bitset>
 #include <cstring>
 #include <fstream>
 #include <string>
@@ -116,6 +117,10 @@ int AtaHardDisk::perform_command() {
             this->r_status &= ~BSY;
         }
         break;
+    case DIAGNOSTICS:
+        this->r_error = 1;
+        this->device_set_signature();
+        break;
     case INIT_DEV_PARAM:
         // update fictive disk geometry with parameters from host
         this->sectors = this->r_sect_count;
@@ -123,9 +128,18 @@ int AtaHardDisk::perform_command() {
         this->r_status &= ~BSY;
         this->update_intrq(1);
         break;
-    case DIAGNOSTICS:
-        this->r_error = 1;
-        this->device_set_signature();
+    case SET_MULTIPLE_MODE: // this command is mandatory for ATA devices
+        if (!this->r_sect_count || this->r_sect_count > 128 ||
+            std::bitset<8>(this->r_sect_count).count() != 1) { // power of two?
+            this->multiple_enabled = false;
+            this->r_error  |= ABRT;
+            this->r_status |= ERR;
+        } else {
+            this->sec_per_block = this->r_sect_count;
+            this->multiple_enabled = true;
+        }
+        this->r_status &= ~BSY;
+        this->update_intrq(1);
         break;
     case FLUSH_CACHE: // used by the XNU kernel driver
         this->r_status &= ~(BSY | DRQ | ERR);
@@ -184,6 +198,7 @@ void AtaHardDisk::prepare_identify_info() {
     std::memset(this->data_buf, 0, sizeof(this->data_buf));
 
     buf_ptr[ 0] = 0x0040; // ATA device, non-removable media, non-removable drive
+    buf_ptr[47] = this->sec_per_block; // block size of READ_MULTIPLE/WRITE_MULTIPLE
     buf_ptr[49] = 0x0200; // report LBA support
 
     buf_ptr[ 1] = this->cylinders;
diff --git a/devices/common/ata/atahd.h b/devices/common/ata/atahd.h
index 07a7ee73f..882505d79 100644
--- a/devices/common/ata/atahd.h
+++ b/devices/common/ata/atahd.h
@@ -69,9 +69,10 @@ class AtaHardDisk : public AtaBaseDevice
     uint8_t     heads;
     uint8_t     sectors;
 
-    char * buffer = new char[1 <<17];
+    uint8_t     sec_per_block    = 8; // sectors per block for READ_MULTIPLE/WRITE_MULTIPLE
+    bool        multiple_enabled = true; // READ_MULTIPLE/WRITE_MULTIPLE enabled
 
-    //uint8_t hd_id_data[ATA_HD_SEC_SIZE] = {};
+    char * buffer = new char[1 <<17];
 };
 
 #endif // ATA_HARD_DISK_H