MOV    A, REG[USB_EP0DATA+wLengthHi] ;
    MOV    [USB_TransferSize], A       ;
.L9:
    MOV    [USB_TransferByteCount], 0  ;
    MOV    [USB_TransferByteCount+1], 0;

    OR     [USB_EP0DataToggle], 1 ; setup EP0 data toggle
    JMP    USB_LoadEndpoint            ;
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_ControlReadDataStage
;
;  DESCRIPTION:   This routine processes the data stage of a control read.  It
;                 must be JUMPed to, not called.  It assumes a tranfer descriptor
;                 has been loaded into the driver USB_CurrentTD
;                 data structure.
;
;-----------------------------------------------------------------------------
 USB_ControlReadDataStage:
    JMP     USB_LoadEndpoint

;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_ControlReadStatusStage
;
;  DESCRIPTION:   This routine processes the status stage of a control read.  It
;                 must be JUMPed to, not called.  It handles short or 0 packet
;                 It assumes a tranfer descriptor has been loaded into the
;                 driver USB_CurrentTD data structure.
;
;-----------------------------------------------------------------------------
 USB_ControlReadStatusStage:
    MOV    A, [USB_LastSize]           ; Get the number of bytes from the last transfer
    ADD    [USB_TransferByteCount + 1], A ; Update the transfer byte count
    ADC    [USB_TransferByteCount], 0  ;
    MOV    A, USB_XFER_STATUS_ACK      ; Return a Status ACK Completion
    CALL   USB_UpdateStatusBlock
    MOV    [USB_TransferType], USB_TRANS_STATE_IDLE
    MOV    A, 0                        ; Count Register
    MOV    X, USB_MODE_STALL_IN_OUT
    JMP    USB_EP0_UPD_MODE_EXIT
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_NoDataStageControlTransfer
;                 USB_InitNoDataStageControlTransfer
;
;  DESCRIPTION:   This routine processes the status stage of a no data control
;                 write.  It must be JUMPed to, not called.
;
;-----------------------------------------------------------------------------
 USB_NoDataStageControlTransfer:
 USB_InitNoDataStageControlTransfer:
    CALL    USB_InitializeStatusBlock

    MOV    [USB_TransferType], USB_TRANS_STATE_NO_DATA_CONTROL

    MOV    A, 0                        ; Count Register
    MOV    X, USB_MODE_STATUS_IN_ONLY
    JMP    USB_EP0_UPD_MODE_EXIT
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_InitControlWrite
;
;  DESCRIPTION:   This routine initializes control write.  It must be JUMPed
;                 to, not called.  It assumes a tranfer descriptor has been loaded
;                 into the driver USB_CurrentTD data structure.
;
;-----------------------------------------------------------------------------
 USB_InitControlWrite:
    MOV     A, [USB_DataSource]        ; Need to make sure the destination is not ROM
    CMP     A, USB_DS_ROM
    JZ      USB_Not_Supported_Local_Drv

    CALL    USB_InitializeStatusBlock

    MOV    [USB_TransferType], USB_TRANS_STATE_CONTROL_WRITE

    OR     [USB_EP0DataToggle], 1 ; setup EP0 data toggle

    MOV    A, 0                        ; Count Register
    MOV    X, USB_MODE_ACK_OUT_STATUS_IN
    JMP    USB_EP0_UPD_MODE_EXIT
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_ControlWriteDataStage
;
;  DESCRIPTION:   This routine processes the data stage of a control
;                 write.  It must be JUMPed to, not called.  It assumes a
;                 tranfer descriptor has been loaded into the driver
;                 USB_CurrentTD data structure.
;
;-----------------------------------------------------------------------------
 USB_ControlWriteDataStage:
    XOR    [USB_EP0DataToggle], 1      ; Update data toggle

    MOV    A,REG[USB_EP0CNT]           ; Get the count

    AND     A, 0x0F
    SUB     A, 2                       ; Count include the two byte checksum

    MOV     [USB_t2], A                ; Assume we have room to receive the whole packet

    MOV     A, 0
    CMP     A, [USB_TransferSize]      ; If the MSB has anything just use the count
    JNZ     .L1

    MOV     A, [USB_t2]
    CMP     A, [USB_TransferSize+1]    ;
    JZ      .L6
.L1:
    JC      .L6
    MOV     [USB_t2], [USB_TransferSize+1]

.L6:
    MOV     A, [USB_t2]
    SUB     [USB_TransferSize+1],A     ; Update the bytes remaining
    SBB     [USB_TransferSize], 0      ;

    ADD     [USB_TransferByteCount + 1], A ; Update the transfer byte count
    ADC     [USB_TransferByteCount], 0 ;

    MOV     X,0                        ; Start the index at 0

.RAM_COPY:
    MOV     A, REG[X+USB_EP0DATA]      ; Get the data
    MVI     [USB_DataPtr+1], A         ; Store the data, bump the destination

    INC     X                          ; Bump the destination offset
    MOV     A,X                        ; Are we done?
    CMP     A, [USB_t2]
    JC      .RAM_COPY                  ; Not done

    MOV    A, 0                        ; Count Register
    MOV    X, USB_MODE_ACK_OUT_STATUS_IN
    JMP    USB_EP0_UPD_MODE_EXIT

; Jump here on data toggle error
.error:
    MOV    [USB_TransferType], USB_TRANS_STATE_IDLE  ; This simply aborts the transfer
    MOV    A, 0                        ; Count Register
    MOV    X, USB_MODE_STALL_IN_OUT
    JMP    USB_EP0_UPD_MODE_EXIT
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_ControlWriteStatusStage
;
;  DESCRIPTION:   This routine processes the status stage of a control
;                 write.  It must be JUMPed to, not called.  It assumes a
;                 tranfer descriptor has been loaded into the driver
;                 USB_CurrentTD data structure.
;
;-----------------------------------------------------------------------------
 USB_ControlWriteStatusStage:
    MOV    A, USB_XFER_STATUS_ACK      ; Return a Status ACK Completion
    CALL   USB_UpdateStatusBlock
    MOV    [USB_TransferType], USB_TRANS_STATE_IDLE  ; The packet is done
    MOV    A, 0                        ; Count Register
    MOV    X, USB_MODE_STALL_IN_OUT
    JMP    USB_EP0_UPD_MODE_EXIT
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_NoDataControlStatusStage
;
;  DESCRIPTION:   This routine processes the status stage of a control
;                 write.  It must be JUMPed to, not called.  It assumes a
;                 tranfer descriptor has been loaded into the driver
;                 USB_CurrentTD data structure.
;
;                 USB Device Addressing happens here because we can't change
;                 the SIE Address before the Status IN is received.
;
;-----------------------------------------------------------------------------
 USB_NoDataControlStatusStage:
    MOV     A, USB_XFER_STATUS_ACK     ; Return a Status ACK Completion
    CALL    USB_UpdateStatusBlock
    ; Dispatch to the proper handler
    CMP     [USB_fDataPending], USB_ADDRESS_CHANGE_PENDING
    JNZ     .L1

    ; USB ADDRESS CHANGE
    MOV     A, [USB_TransferBuffer]    ; Get the pending data

    OR      A, USB_ADDR_ENABLE         ; Set the enable bit
    MOV     REG[USB_ADDR], A           ; Update the SIE address
    JMP     .EXIT
.L1:

.EXIT:
    MOV     [USB_fDataPending], 0      ; Clear data pending

    MOV    A, 0                        ; Count Register
    MOV    X, USB_MODE_STATUS_IN_ONLY  ; Wait for the next SETUP
    JMP    USB_EP0_UPD_MODE_EXIT
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_NoDataControlError
;
;  DESCRIPTION:   This routine handles the condition when we expected a
;                 status IN, but receive an OUT
;
;-----------------------------------------------------------------------------
 USB_NoDataControlError:
    MOV    A, USB_XFER_ERROR           ; Return Transaction Error
    CALL   USB_UpdateStatusBlock
    MOV    A, 0                        ; Count Register
    MOV    X, USB_MODE_STALL_IN_OUT    ; Set the mode register
    JMP    USB_EP0_UPD_MODE_EXIT
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_LoadEndpoint
;
;  DESCRIPTION:  Load the endpoint buffer with the next data for the control
;                transfer.  It assumes a tranfer descriptor has been loaded
;                into the driver USB_CurrentTD data structure.
;
;-----------------------------------------------------------------------------
USB_LoadEndpoint:
    MOV     A, [USB_LastSize]          ; Get the number of bytes from the last transfer
    ADD     [USB_TransferByteCount + 1], A ; Update the transfer byte count
    ADC     [USB_TransferByteCount], 0 ;
    MOV     A,  [USB_TransferSize]     ; Check to see if we have any
    OR      A,  [USB_TransferSize+1]   ;  more data to send
    JNZ     .cont                      ; Jump if we have to send more data

; Flow here if there is no more data to send
    CMP     [USB_LastSize], 8          ; Was it a full packet?
    JZ      .START_TRANSFER            ; Jump if it was full (need to send a zero length)

; Flow here if we are entering the status stage
    MOV    A, 0                        ; Count Register
    MOV    [USB_LastSize], A           ; Clear the byte count
    MOV    X, USB_MODE_STATUS_OUT_ONLY ; Only ACK the Status Out
    JMP    USB_EP0_UPD_MODE_EXIT

; Jump here to determine how many bytes should we transfer
.cont:
    CMP     [USB_TransferSize], 0      ; Check the MSB
    JNZ     .L1

    CMP     [USB_TransferSize+1], 8    ; Check the LSB
    JNC     .L1

    MOV     A,[USB_TransferSize+1]     ; Transfer all the remaing data
    JMP     .L3

.L1:
    MOV     A, 8                       ; Just transfer the next 8 bytes

.L3:

    SUB     [USB_TransferSize+1],A     ; Update the bytes remaining
    SBB     [USB_TransferSize],0

    MOV     [USB_t2],A                 ; Save the count
    MOV     X,0

    CMP     [USB_DataSource],USB_DS_ROM  ; RAM or ROM copy?
    JNZ     .RAM_COPY

; Copy data from a ROM source
.ROM_COPY:
    PUSH    X                          ; Save the destination offset
    MOV     A,[USB_DataPtr]            ; Get the transfer source MSB
    MOV     X,[USB_DataPtr+1]          ; Set the transfer source LSB
    INC     [USB_DataPtr+1]            ; Increment the data pointer
    ADC     [USB_DataPtr], 0           ;   MSB if necessary

    ROMX                               ; Get the data byte

    POP     X                          ; Get the destination offset
    MOV     REG[X + USB_EP0DATA], A    ; Load the data
    INC     X                          ; Bump the destination offset
    MOV     A,X                        ; Are we done?
    CMP     A, [USB_t2]
    JC      .ROM_COPY                  ; Not done
    JMP     .START_TRANSFER            ; Otherwise go start the transfer

; Copy data from a RAM source
.RAM_COPY:
    MVI     A, [USB_DataPtr+1]         ; Get the data, bump the source

    MOV     REG[X +USB_EP0DATA], A     ; Load the data
    INC     X                          ; Bump the destination offset
    MOV     A,X                        ; Are we done?
    CMP     A, [USB_t2]
    JC      .RAM_COPY                  ; Not done

;; Set up the IN transfer count/mode/etc
;    A contains the byte count
.START_TRANSFER:
    MOV     [USB_LastSize], A          ; Save the packet size

    MOV     A, 1
    AND     A, [USB_EP0DataToggle]
    JZ      .BYPASS_T1
    MOV     A, USB_CNT_TOGGLE          ; Or T1 in the data toggle

.BYPASS_T1:
    OR      A, [USB_LastSize]
    XOR     [USB_EP0DataToggle], 1     ; Update the data toggle for next time

    MOV    X, USB_MODE_ACK_IN_STATUS_OUT  ; Set the mode register
    JMP    USB_EP0_UPD_MODE_EXIT
;-----------------------------------------------------------------------------
;  FUNCTION NAME: USB_GetTableEntry
;
;  DESCRIPTION:   Copies a TD to the USB_CurrentTD data structure.
;
;-----------------------------------------------------------------------------
EXPORT USB_GetTableEntry
USB_GetTableEntry:

    INC     X                          ; Point to the first table entry
    ADC     A, 0                       ;