Fixes for READ_MULTIPLE and WRITE_MULTIPLE commands

We may be transferring less data than can fit in a chunk, so we need
to ensure that xfer_cnt is clamped to 0 when transferring the last
chunk (otherwise it remains negative, and has_data() will return true).

It's also possible that the transfer size is bigger than a chunk but not
an even multiple of the chunk size, so we need to ensure that we don't
try to transfer a whole chunk in the last iteration.

More correctly initialize the device identification struct, to report
the maximum (word 47) and current (word 59) number of blocks that can be
transferred with READ_MULTIPLE and WRITE_MULTIPLE commands.

Fix post_xfer_action to write the actual data size that was written,
as opposed to an entire chunk (which may be larger).

Author: mihaip

devices/common/ata/atabasedevice.cpp

@@ -70,10 +70,11 @@ uint16_t AtaBaseDevice::read(const uint8_t reg_addr) {
             this->chunk_cnt -= 2;
             if (this->chunk_cnt <= 0) {
                 this->xfer_cnt -= this->chunk_size;
-                if (this->xfer_cnt <= 0)
+                if (this->xfer_cnt <= 0) {
+                    this->xfer_cnt = 0;
                     this->r_status &= ~DRQ;
-                else {
-                    this->chunk_cnt = this->chunk_size;
+                } else {
+                    this->chunk_cnt = std::min(this->xfer_cnt, this->chunk_size);
                     this->update_intrq(1);
                 }
             }
@@ -114,12 +115,13 @@ void AtaBaseDevice::write(const uint8_t reg_addr, const uint16_t value) {
                 this->post_xfer_action();
                 this->xfer_cnt -= this->chunk_size;
                 if (this->xfer_cnt <= 0) { // transfer complete?
+                    this->xfer_cnt = 0;
                     this->r_status &= ~DRQ;
                     this->r_status &= ~BSY;
                     this->update_intrq(1);
                 } else {
                     this->cur_data_ptr = this->data_ptr;
-                    this->chunk_cnt = this->chunk_size;
+                    this->chunk_cnt = std::min(this->xfer_cnt, this->chunk_size);
                     this->signal_data_ready();
                 }
             }

devices/common/ata/atahd.cpp

@@ -106,7 +106,7 @@ int AtaHardDisk::perform_command() {
             uint32_t ints_size = ATA_HD_SEC_SIZE;
             if (this->r_command == READ_MULTIPLE) {
                 if (this->sec_per_block == 0) {
-                    LOG_F(ERROR, "%s: READ MULTIPLE with SET MULTIPLE==0", this->name.c_str());
+                    LOG_F(ERROR, "%s: cannot do a READ_MULTIPLE with unset sec_per_block", this->name.c_str());
                     this->r_status |= ERR;
                     this->r_status &= ~BSY;
                     break;
@@ -131,7 +131,7 @@ int AtaHardDisk::perform_command() {
             uint32_t ints_size = ATA_HD_SEC_SIZE;
             if (this->r_command == WRITE_MULTIPLE) {
                 if (this->sec_per_block == 0) {
-                    LOG_F(ERROR, "%s: WRITE MULTIPLE with SET MULTIPLE==0", this->name.c_str());
+                    LOG_F(ERROR, "%s: cannot do a WRITE_MULTIPLE with unset sec_per_block", this->name.c_str());
                     this->r_status |= ERR;
                     this->r_status &= ~BSY;
                     break;
@@ -140,8 +140,9 @@ int AtaHardDisk::perform_command() {
             }
             this->prepare_xfer(xfer_size, ints_size);
             this->post_xfer_action = [this]() {
-                this->hdd_img.write(this->data_ptr, this->cur_fpos, this->chunk_size);
-                this->cur_fpos += this->chunk_size;
+                uint64_t write_len = (this->cur_data_ptr - this->data_ptr) * sizeof(this->data_ptr[0]);
+                this->hdd_img.write(this->data_ptr, this->cur_fpos, write_len);
+                this->cur_fpos += write_len;
             };
             this->r_status |= DRQ;
             this->r_status &= ~BSY;
@@ -168,10 +169,12 @@ int AtaHardDisk::perform_command() {
     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?
+            LOG_F(ERROR, "%s: SET_MULTIPLE_MODE not suported, invalid r_sect_count (%d)", this->name.c_str(), this->r_sect_count);
             this->multiple_enabled = false;
             this->r_error  |= ABRT;
             this->r_status |= ERR;
         } else {
+            LOG_F(INFO, "%s: SET_MULTIPLE_MODE, r_sect_count=%d", this->name.c_str(), this->r_sect_count);
             this->sec_per_block = this->r_sect_count;
             this->multiple_enabled = true;
         }
@@ -235,9 +238,21 @@ 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
 
+    // Maximum number of logical sectors per data block that the device supports
+    // for READ_MULTIPLE/WRITE_MULTIPLE commands.
+    const int max_sec_per_block = 8;
+    if (max_sec_per_block > 1) {
+        buf_ptr[47] = 0x8000 | max_sec_per_block;
+    }
+    // If bit 8 of word 59 is set to one, then bits 7:0 indicate the number of
+    // logical sectors that shall be transferred per data block for
+    // READ_MULTIPLE/WRITE_MULTIPLE commands.
+    if (this->sec_per_block) {
+        buf_ptr[59] = 0x100 | this->sec_per_block;
+    }
+
     buf_ptr[ 1] = this->cylinders;
     buf_ptr[ 3] = this->heads;
     buf_ptr[ 6] = this->sectors;

devices/common/ata/atahd.h

@@ -69,8 +69,8 @@ class AtaHardDisk : public AtaBaseDevice
     uint8_t     heads;
     uint8_t     sectors;
 
-    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     sec_per_block    = 0; // sectors per block for READ_MULTIPLE/WRITE_MULTIPLE
+    bool        multiple_enabled = false; // READ_MULTIPLE/WRITE_MULTIPLE enabled
 
     char * buffer = new char[1 <<17];
 };