From dc0f8cb0cd62e08bc940cf242844bdaa107c9f5b Mon Sep 17 00:00:00 2001
From: Patrik Axelsson <patrik.bo@gmail.com>
Date: Sat, 4 Mar 2023 01:43:07 +0100
Subject: [PATCH] Add NSD and TD64 command support for 4GB+ SD cards

---
 examples/spisd/device.c | 72 +++++++++++++++++++++++++++++++++++++++--
 1 file changed, 69 insertions(+), 3 deletions(-)

diff --git a/examples/spisd/device.c b/examples/spisd/device.c
index 9ede9f5..6d6d398 100644
--- a/examples/spisd/device.c
+++ b/examples/spisd/device.c
@@ -17,6 +17,7 @@
 #include <libraries/dos.h>
 #include <devices/timer.h>
 #include <devices/trackdisk.h>
+#include <devices/newstyle.h>
 #include <proto/exec.h>
 #include <proto/alib.h>
 
@@ -36,6 +37,10 @@
 #define SIGF_OP_REQUEST (1 << SIGB_OP_REQUEST)
 #define SIGF_OP_TIMER (1 << SIGB_TIMER)
 
+// How much of struct NSDeviceQueryResult we use/need. It could be extended
+// and we don't want that to change the behaviour of the code.
+#define NSD_QUERY_RESULT_LENGTH_REQUIRED 16
+
 struct ExecBase *SysBase;
 static BPTR saved_seg_list;
 static struct timerequest tr;
@@ -99,6 +104,11 @@ static void handle_changed()
         Cause((struct Interrupt *)change_int->io_Data);
 }
 
+static uint32_t offset_to_sd_sectors(uint32_t high_offset, uint32_t low_offset)
+{
+    return (high_offset << (32 - SD_SECTOR_SHIFT)) | (low_offset >> SD_SECTOR_SHIFT);
+}
+
 static void process_request(struct IOStdReq *ior)
 {
     if (!card_present)
@@ -115,14 +125,22 @@ static void process_request(struct IOStdReq *ior)
 
         case TD_FORMAT:
         case CMD_WRITE:
-            if (sd_write((uint8_t *)ior->io_Data, ior->io_Offset >> SD_SECTOR_SHIFT, ior->io_Length >> SD_SECTOR_SHIFT) == 0)
+            ior->io_Actual = 0;
+        case TD_FORMAT64:
+        case TD_WRITE64:
+        case NSCMD_TD_FORMAT64:
+        case NSCMD_TD_WRITE64:
+            if (sd_write((uint8_t *)ior->io_Data, offset_to_sd_sectors(ior->io_Actual, ior->io_Offset), ior->io_Length >> SD_SECTOR_SHIFT) == 0)
                 ior->io_Actual = ior->io_Length;
             else
                 ior->io_Error = TDERR_NotSpecified;
             break;
 
         case CMD_READ:
-            if (sd_read((uint8_t *)ior->io_Data, ior->io_Offset >> SD_SECTOR_SHIFT, ior->io_Length >> SD_SECTOR_SHIFT) == 0)
+            ior->io_Actual = 0;
+        case TD_READ64:
+        case NSCMD_TD_READ64:
+            if (sd_read((uint8_t *)ior->io_Data, offset_to_sd_sectors(ior->io_Actual, ior->io_Offset), ior->io_Length >> SD_SECTOR_SHIFT) == 0)
                 ior->io_Actual = ior->io_Length;
             else
                 ior->io_Error = TDERR_NotSpecified;
@@ -167,13 +185,39 @@ static void change_isr()
     Signal(task, SIGF_CARD_CHANGE);
 }
 
+static const UWORD supported_commands[] =
+{
+    CMD_RESET,
+    CMD_READ,
+    CMD_WRITE,
+    CMD_UPDATE,
+    CMD_CLEAR,
+    TD_MOTOR,
+    TD_FORMAT,
+    TD_REMOVE,
+    TD_CHANGENUM,
+    TD_CHANGESTATE,
+    TD_PROTSTATUS,
+    TD_GETDRIVETYPE,
+    TD_ADDCHANGEINT,
+    TD_REMCHANGEINT,
+    TD_GETGEOMETRY,
+    TD_READ64,
+    TD_WRITE64,
+    TD_FORMAT64,
+    NSCMD_DEVICEQUERY,
+    NSCMD_TD_READ64,
+    NSCMD_TD_WRITE64,
+    NSCMD_TD_FORMAT64,
+    0
+};
+
 static void begin_io(__reg("a6") struct Library *dev, __reg("a1") struct IOStdReq *ior)
 {
     if (!ior)
         return;
 
     ior->io_Error = 0;
-    ior->io_Actual = 0;
 
     switch (ior->io_Command)
     {
@@ -182,6 +226,7 @@ static void begin_io(__reg("a6") struct Library *dev, __reg("a1") struct IOStdRe
     case CMD_UPDATE:
     case TD_MOTOR:
     case TD_PROTSTATUS:
+        ior->io_Actual = 0;
         break;
 
     case TD_CHANGESTATE:
@@ -216,10 +261,31 @@ static void begin_io(__reg("a6") struct Library *dev, __reg("a1") struct IOStdRe
             change_int = NULL;
         break;
 
+    case NSCMD_DEVICEQUERY:
+        if (ior->io_Length >= NSD_QUERY_RESULT_LENGTH_REQUIRED)
+        {
+            struct NSDeviceQueryResult *result = ior->io_Data;
+            result->nsdqr_DevQueryFormat = 0;
+            result->nsdqr_SizeAvailable = NSD_QUERY_RESULT_LENGTH_REQUIRED;
+            result->nsdqr_DeviceType = NSDEVTYPE_TRACKDISK;
+            result->nsdqr_DeviceSubType = 0;
+            result->nsdqr_SupportedCommands = supported_commands;
+            ior->io_Actual = NSD_QUERY_RESULT_LENGTH_REQUIRED;
+        }
+        else {
+            ior->io_Error = IOERR_BADLENGTH;
+        }
+        break;
+
     case TD_GETGEOMETRY:
     case TD_FORMAT:
     case CMD_WRITE:
     case CMD_READ:
+    case TD_READ64:
+    case TD_WRITE64:
+    case NSCMD_TD_READ64:
+    case NSCMD_TD_WRITE64:
+    case NSCMD_TD_FORMAT64:
         PutMsg(&mp, (struct Message *)&ior->io_Message);
         ior->io_Flags &= ~IOF_QUICK;
         ior = NULL;