readres3 assembly rewrite

sw/bootloader/devices/sd_card.c

@@ -172,6 +172,7 @@ void SD_readRes2(uint8_t *res)
 /*******************************************************************************
  Read R3 from SD card
 *******************************************************************************/
+/*
 void SD_readRes3(uint8_t *res)
 {
     // read response 1 in R3
@@ -183,6 +184,7 @@ void SD_readRes3(uint8_t *res)
     // read remaining bytes
     SD_readBytes(res + 1, R3_BYTES);
 }
+*/
 
 /*******************************************************************************
  Read R7 from SD card

sw/bootloader/devices/sd_card_asm.s

@@ -1,12 +1,15 @@
 .export _SD_command
 .export _SD_readRes1
 .export _SD_readRes2
+.export _SD_readRes3
 .export _SD_readBytes
 
 .importzp sp, ptr1
 
 .autoimport on
 
+.MACPACK generic
+
 ; void SD_command(uint8_t cmd, uint32_t arg, uint8_t crc)
 
 ; The plan: push crc to stack, load arg into tmp1 through 4
@@ -91,4 +94,30 @@ read:
         bne     read            ; and if x is zero we are done
         rts
 
+.endproc
+
+; void SD_readRes3(uint8_t *res)
+
+.proc   _SD_readRes3:   near
+
+        sta     ptr1            ; store res in ptr1
+        stx     ptr1 + 1
+
+        jsr     _SD_readRes1    ; read respopnse 1 in R3
+        cmp     #$02            ; if error reading R1, return
+        bge     @L1
+
+        inc     ptr1            ; read remaining bytes
+        bne     @L2
+        inc     ptr1
+@L2:    lda     ptr1            ; push low byte
+        sta     (sp)
+        ldy     #$01
+        lda     ptr1 + 1        ; push high byte
+        sta     (sp),y
+        lda     #$04            ; R3_BYTES
+        jsr     _SD_readBytes
+
+@L1:    rts
+
 .endproc