Index: ../firmware/usbstack/usb_storage.c
===================================================================
--- ../firmware/usbstack/usb_storage.c	(revision 16387)
+++ ../firmware/usbstack/usb_storage.c	(working copy)
@@ -20,7 +20,7 @@
 #include "system.h"
 #include "usb_core.h"
 #include "usb_drv.h"
-//#define LOGF_ENABLE
+#define LOGF_ENABLE
 #include "logf.h"
 #include "ata.h"
 #include "hotswap.h"
@@ -165,6 +165,9 @@
 static unsigned char _transfer_buffer[2*BUFFER_SIZE] __attribute((aligned (4096)));
 static unsigned char* transfer_buffer;
 
+static unsigned char _verify_buffer[2*BUFFER_SIZE] __attribute((aligned (4096)));
+static unsigned char* verify_buffer;
+
 static struct inquiry_data* inquiry;
 static unsigned char __inquiry[CACHEALIGN_UP(sizeof(struct inquiry_data))] CACHEALIGN_ATTR;
 
@@ -228,6 +231,7 @@
 void usb_storage_init(void)
 {
     transfer_buffer = (void*)UNCACHED_ADDR(&_transfer_buffer);
+    verify_buffer = (void*)UNCACHED_ADDR(&_verify_buffer);
     inquiry = (void*)UNCACHED_ADDR(&__inquiry);
     capacity_data = (void*)UNCACHED_ADDR(&__capacity_data);
     format_capacity_data = (void*)UNCACHED_ADDR(&__format_capacity_data);
@@ -268,6 +272,7 @@
                 }
 
                 /* Now write the data that just came in, while the host is sending the next bit */
+                int irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
                 int result = ata_write_sectors(IF_MV2(current_cmd.lun,)
                                                current_cmd.sector, MIN(BUFFER_SIZE/SECTOR_SIZE,current_cmd.count),
                                                current_cmd.data[current_cmd.data_select]);
@@ -277,7 +282,27 @@
                     cur_sense_data.asc=ASC_WRITE_ERROR;
                     break;
                 }
+                ata_read_sectors(IF_MV2(current_cmd.lun,)
+                                 current_cmd.sector, MIN(BUFFER_SIZE/SECTOR_SIZE,current_cmd.count),
+                                 verify_buffer);
+                set_irq_level(irq_level);
+                if(memcmp(verify_buffer,current_cmd.data[current_cmd.data_select],MIN(BUFFER_SIZE/SECTOR_SIZE,current_cmd.count)*SECTOR_SIZE)) {
+                    logf("Data does not match !");
+                    int i;
+                    for(i=0;i<MIN(BUFFER_SIZE/SECTOR_SIZE,current_cmd.count)*SECTOR_SIZE;i+=16)
+                    {
+                        if(memcmp(&verify_buffer[i],&(current_cmd.data[current_cmd.data_select][i]),16)){
+                            logf("Offset:%X",i);
+                            int j;
+                            for(j=0;j<4;j++)
+                            {
+                                logf("%X ATA:%X USB:%X",j,*(unsigned int *)(&verify_buffer[i+j]),*(unsigned int *)(&current_cmd.data[current_cmd.data_select][i+j]));
+                            }
+                        }
+                    }
+                }
 
+
                 if(next_count==0) {
                     send_csw(SCSI_STATUS_GOOD);
                 }