;NavNet packet routines

 include "ti83plus.inc"
 include "equates.inc" 
 include "usb8x.inc"
 include "settings.inc"
 SEGMENT Main
 GLOBALS ON

 PUBLIC HandleServicePacket,CalculateCRC,SendServiceDisconnect,DoLog2
 EXTERN MakeNCall,RequestInputData,ThrowNavNetError
 EXTERN ListenToEcho,GetScreenshot,HandleAddressAssignment,RetrieveDeviceInformation
 EXTERN FileManagement,OSInstallation
 EXTERN GetScreenshotData,DispHexHL,DispHexA,IPutC

HandleServicePacket:
;Handle packet at 83:4100h with service HL
;B is the sequence number of the packet we're handling
       set sendServiceDisconnect,(iy+nspire8xFlags)
       ld a,b
       ld (tempSeqID),a
       bit receivingScreenshot,(iy+nspire8xFlags)
       jr nz,GetScreenshotData
       ld ix,serviceHandlerTable
hspFindLoop:
       ld e,(ix+0)
       ld d,(ix+1)
       ld a,d
       or e
       ld a,errUnknownServicePacket
       jr z,ThrowNavNetError
       B_CALL cphlde
       jr nz,$F
       ld l,(ix+2)
       ld h,(ix+3)
       jp (hl)
$$:    ld de,4
       add ix,de
       jr hspFindLoop

serviceHandlerTable:
       DW 4003h
       DW HandleAddressAssignment
       DW 4020h
       DW RetrieveDeviceInformation
       DW 4024h
       DW GetScreenshot
       DW 4002h
       DW ListenToEcho
       DW 4060h
       DW FileManagement
       DW 4080h
       DW OSInstallation
       DW 00FFh
       DW AcknowledgementReceived
       DW 0000h

GetPacketByte:
;Get a byte from the current packet at the specified offset.
;Inputs:
;      BC: offset
;Outputs:
;      A: byte
       push ix
       push bc
       ld a,03h
       out (5),a
       ld ix,0D000h+512
       add ix,bc
       ld b,(ix+0)
       xor a
       out (5),a
       ld a,b
       pop bc
       pop ix
       ret

GetPacketWord:
;Get a word from the current packet at the specified offset.
;Inputs:
;      BC: offset
;Outputs:
;      DE: word
       push ix
       push bc
       ld a,03h
       out (5),a
       ld ix,0D000h+512
       add ix,bc
       ld e,(ix+0)
       ld d,(ix+1)
       xor a
       out (5),a
       pop bc
       pop ix
       ret

SendNavNetPacket:
;Sends a NavNet packet over the wire.
;Inputs:      HL: source service ID
;             DE: destination service ID
;             B:  acknowledgement ID (AK from the specification)
;             IX => data to send (if any)
;             C: number of bytes to send (0 for none)
;Outputs:     A is the sequence ID used for this packet
       push de
       push hl
       push bc
       ld hl,(NavNetMemPtr)
       ld bc,offPacketBuffer+16
       add hl,bc
       pop bc
       ;Copy data to packet buffer
       ex de,hl
       push ix
       pop hl
       push bc
       ld a,c
       or c
       jr z,$F
       ld b,0
       ldir
$$:    pop bc
       pop hl
       pop de
       push bc
       ld ix,(NavNetMemPtr)
       ld bc,offPacketBuffer
       add ix,bc
       pop bc
       ld (ix+0),54h
       ld (ix+1),0FDh
       ld (ix+2),64h
       ld (ix+3),00h
       ld (ix+4),h
       ld (ix+5),l
       ld (ix+6),64h
       ld (ix+7),01h
       ld (ix+8),d
       ld (ix+9),e
       ld (ix+12),c
       ld (ix+13),b
       push ix
       ld a,b
       cp 0Ah
       ld ix,(NavNetMemPtr)
       ld a,(ix+offSequenceNumber)
       jr nz,$F
       ;Use the sequence number from the other packet instead
       ld a,(tempSeqID)
$$:    pop ix
       ld (ix+14),a
       ld (tempSeqID),a
       ;Calculate checksum of the data part
       push bc
       push ix
       pop hl
       ld de,16
       add hl,de
       ld b,c
       call CalculateCRC
       ld (ix+10),d
       ld (ix+11),e
       pop bc
       ;Calculate checksum of the header
       push ix
       pop hl
       ld b,15
       xor a
$$:    ld d,(hl)
       add a,d
       inc hl
       djnz $B
       ld (ix+15),a
       ;Actually send the packet
       ld a,c
       ld hl,(NavNetMemPtr)
       ld bc,offPacketBuffer
       add hl,bc
       ld ix,(NavNetMemPtr)
       ld c,(ix+offOutPipe)
;       add a,16
;       ld b,a
;       N_CALL SendData
       push hl
       ld hl,16
       ld d,0
       ld e,a
       add hl,de
       ex de,hl
       pop hl
       call SendData
       ld a,errSendPacketData
       jr c,ThrowNavNetError
       ld a,(tempSeqID)
       or a
       ret
;We must replace usb8x's SendData routine because we need to send more than 255 bytes at once.
;This should probably be added to usb8x.
SendData:
;Sends bulk data to a connected USB device.
;Inputs:      HL => data to send
;             DE: number of bytes to send
;             C: pipe/endpoint number to send data on
       DISAVE
       call SendDataSafe
       EIRESTORE
       ret
SendDataSafe:
       push bc
       call SendDataDo
       pop bc
       ret c
       push af
       ld b,c
       ld c,8
       call SendOutCmdNoWait
       pop af
       ret
SendDataDo:
       LOG SendData,c
       ld a,c
       out (8Eh),a
       push hl
       push de
       ld ix,tempByte
       ld hl,(usb8xMemPtr)
       ld de,offPacketSizeTable
       add hl,de
       ld e,c
       ld d,0
       add hl,de
       ld d,(hl)
       ld (ix+0),d
       pop de
       pop hl
       ;(ix+0): max packet size
       ;DE is bytes left to send
       ld a,c
       add a,0A0h
       ld c,a
SendLoop:
       ld b,(ix+0)
       ;See if we have at least B bytes to send
       push bc
       ld c,b
       ld b,0
       ex de,hl
       or a
       sbc hl,bc
       ex de,hl
       pop bc
       ;NC if we have at least B bytes to send
       jr nc,$F
       push bc
       ex de,hl
       ld b,0
       ld c,(ix+0)
       add hl,bc
       ex de,hl
       pop bc
       ld b,e
       ld de,0
$$:    LOG DataStart,b
sdLoop:
       ld a,(hl)
       LOG Data,a
       out (c),a
       inc hl
       djnz sdLoop
       push bc
       ld a,c
       sub 0A0h
       ld b,a
       ld c,1
       call SendOutCmdWait
       pop bc
       LOG DataEnd
       ret c
       ld a,d
       or e
       jr nz,SendLoop
       xor a
       ret
SendOutCmdNoWait:
       ld a,b
       out (8Eh),a
       ld a,c
       out (91h),a
CheckPort91:
       push af
       in a,(91h)
       bit 2,a
       jr nz,usb8xErrorP1
       bit 4,a
       jr nz,usb8xErrorP1
       pop af
       or a
       ret
usb8xErrorP3:
       pop hl
       pop hl
usb8xErrorP1:
       pop hl
       scf
       ret
WaitPort82U:
       push af
       push de
       push hl
       push bc
       ld hl,(usb8xMemPtr)
       ld de,offFlags
       add hl,de
       ld de,0FFFFh
$$:    ld a,d
       or e
       jr z,usb8xErrorP3
       dec de
       bit flagPort82,(hl)
       jr nz,$F
       in a,(82h)
       or a
       jr z,$B
$$:    pop bc
       pop hl
       pop de
       pop af
       or a
       ret
ResetPort82Flag:
       push hl
       push de
       ld hl,(usb8xMemPtr)
       ld de,offFlags
       add hl,de
       res flagPort82,(hl)
       pop de
       pop hl
       ret
SendOutCmdWait:
       call ResetPort82Flag
       ld a,b
       out (8Eh),a
       ld a,c
       out (91h),a
       call WaitPort82U
       ret c
       jr CheckPort91

  IF LOGGING_ENABLED=1
DoLog2:
       push   ix
       push   hl
       push   de
       push   af
       push   bc
;       DISAVE
       call   DoLogDo
;       EIRESTORE                 
       pop    bc
       pop    af
       pop    de
       pop    hl
       pop    ix
       ret

DoLogDo:
       ld     hl,(usb8xMemPtr)
       ld     de,offLogPage
       add    hl,de
       ld     a,(hl)
       or     a
       ret    z
       inc    hl
       ld     e,(hl)
       inc    hl
       ld     d,(hl)
       ex     de,hl                ;HL = logptr
       out    (7),a                ;set log ram page
       ld     a,b
       cp     logData
       jr     z,DoLogData
       cp     logDataEnd
       jr     z,DoLogDataEnd
       cp     logDataStart
       jr     nz,$f
       in     a,(7)
       push   af
       ld     a,81h
       out    (7),a
       ld     ix,(usb8xMemPtr)
       ld     (ix+offLogDataAddr),l
       ld     (ix+offLogDataAddr+1),h
       pop    af
       out    (7),a
$$:
       ld     (hl),b
       inc    hl
       ld     (hl),c
       inc    hl
       ld     (hl),0
       inc    hl
       ld     (hl),0
       inc    hl
DoLogCont:
       ld     a,81h
       out    (7),a
       ex     de,hl
       ld     (hl),d
       dec    hl
       ld     (hl),e
       ret
DoLogData:
;If we've already stored 32 bytes of data, time to start a new data log entry

       jr     LogDataOK


       push   bc
       push   hl

       in     a,(7)
       push   af
       ld     a,81h
       out    (7),a
       ld     ix,(usb8xMemPtr)
       ld     c,(ix+offLogDataAddr)          
       ld     b,(ix+offLogDataAddr+1)            ;BC -> data start address
       pop    af
       out    (7),a
       
       or     a
       sbc    hl,bc                              ;How much data has been stored so far?
       ld     bc,68
       or     a
       sbc    hl,bc                              ;If it's 32 bytes, split up into two log entries
       pop    hl
       pop    bc
       jr     nz,LogDataOK
       push   bc                                 ;Start new entry
       ld     a,81h
       out    (7),a
       ex     de,hl
       ld     (hl),d
       dec    hl
       ld     (hl),e

       ld     b,logDataEnd               
       ld     c,0
       call   DoLogDo                     ;End the current data entry
       ld     b,logDataStart
       ld     c,0
       call   DoLogDo                     ;And start the new one
       pop    bc
       jp     DoLogDo                     ;Restore the data


LogDataOK:
       ld     (hl),c
       inc    hl
       jr     DoLogCont

DoLogDataEnd:
       ld     a,l
       and    3
       jr     z,$f
       ld     (hl),0
       inc    hl
       jr     DoLogDataEnd
$$:                                       ;Update DataEnd and DataStart with each other's addresses
       ld     (hl),b                      ;DataEnd
       inc    hl
       inc    hl
       in     a,(7)
       push   af
       ld     a,81h
       out    (7),a
       ld     ix,(usb8xMemPtr)
       ld     c,(ix+offLogDataAddr)          ;DataStart Address
       ld     b,(ix+offLogDataAddr+1)
       pop    af
       out    (7),a

       ld     (hl),c
       inc    hl
       ld     (hl),b
       inc    hl                          ;HL -> next log address
       inc    bc
       inc    bc                          ;BC -> DataStart's data2
       ld     a,l
       ld     (bc),a
       inc    bc
       ld     a,h
       ld     (bc),a
       jr     DoLogCont

 ENDIF

CalculateCRC:
;Calculates a CRC checksum of specified bytes.
;Inputs:
;      HL => data to checksum
;      B: number of bytes
;Outputs:
;      DE: CRC checksum
       ld de,0
       ld a,b
       or a
       ret z
       ;DE = acc
       ld de,0
crcChecksumLoop:
       push bc
       ld a,(hl)
       inc hl
       push hl
       ;A = byte
       ;first = (ord(byte) << 8) | acc >> 8
       ld h,a
       ld l,0
       ld a,l
       or d
       ld l,a
       ld (CRCfirst),hl
       ;acc = acc & 0xFF
       ld d,0
       ;second = (((acc & 0xF) << 4) ^ acc) << 8
       ld a,e
       and 0Fh
       ld h,0
       ld l,a
       add hl,hl
       add hl,hl
       add hl,hl
       add hl,hl
       ld a,d
       xor h
       ld h,a
       ld a,e
       xor l
       ld l,a
       ld h,l
       ld l,0
       ld (CRCsecond),hl
       ;third = second >> 5
       srl h
       rr l
       srl h
       rr l
       srl h
       rr l
       srl h
       rr l
       srl h
       rr l
       ld (CRCthird),hl
       ;acc = third >> 7
       srl h
       rr l
       srl h
       rr l
       srl h
       rr l
       srl h
       rr l
       srl h
       rr l
       srl h
       rr l
       srl h
       rr l
       ld d,h
       ld e,l
       ;acc = (acc ^ first ^ second ^ third) & 0xFFFF
       ld hl,(CRCfirst)
       ld a,d
       xor h
       ld d,a
       ld a,e
       xor l
       ld e,a
       ld hl,(CRCsecond)
       ld a,d
       xor h
       ld d,a
       ld a,e
       xor l
       ld e,a
       ld hl,(CRCthird)
       ld a,d
       xor h
       ld d,a
       ld a,e
       xor l
       ld e,a
       pop hl
       pop bc
       djnz crcChecksumLoop
       ret

AcknowledgementReceived:
       ld a,03h
       out (5),a
       ld ix,0D000h+512
       ld h,(ix+16)
       ld l,(ix+17)
       xor a
       out (5),a
       ;HL is the service ID that the device is acknowledging
       ret

SendAcknowledgement:
;Sends acknowledgement packet for the device's response to our original request.
;Inputs:      DE: destination service ID (i.e. what we're handling)
;Outputs:     Returns carry flag set if problems
       ld b,0Ah
       ld ix,(NavNetMemPtr)
       ld l,(ix+offServiceID)
       ld h,(ix+offServiceID+1)
       ld ix,OP1
       ld (ix+0),h
       ld (ix+1),l
       ld hl,00FFh
       ld c,2
       call SendNavNetPacket
       ret nc
       ld a,errSendAcknowledgement
       jr ThrowNavNetError

SendServiceDisconnect:
;Sends service disconnect NavNet request.
;Outputs:     Returns carry flag set if problems
       ld b,0
       jr _FinishHandlingPacket

FinishHandlingPacket:
;Finish processing a NavNet request.
;Inputs:      DE: service ID we're finishing handling
;             B:  event code to raise (or 0 for none)
;             A:  sequence ID to go along with the event
;Outputs:     Returns carry flag set if problems
       ld ix,(NavNetMemPtr)
       bit sendServiceDisconnect,(iy+nspire8xFlags)
       jr z,$F
_FinishHandlingPacket:
       ;Disconnect from this service
       ld ix,(NavNetMemPtr)
       push bc
       push af
       ;NOTE: We're ignoring the device's acknowledgement of this
       ld l,(ix+offServiceID)
       ld h,(ix+offServiceID+1)
       ld b,0
       ld ix,OP1
       ld (ix+0),h
       ld (ix+1),l
       ld hl,40DEh
       ld c,2
       call SendNavNetPacket
       pop bc
       ld a,b
       pop bc
       jr nc,fhpSendSuccessful
       ld a,errServiceDisconnectRequest
       jr ThrowNavNetError
fhpSendSuccessful:
       ;Increase our local service ID
       ld ix,(NavNetMemPtr)
       ld e,(ix+offServiceID)
       ld d,(ix+offServiceID+1)
       inc de
       ld (ix+offServiceID),e
       ld (ix+offServiceID+1),d
$$:    ;Queue this event and sequence ID so the original request knows it's done
       ld l,(ix+offEventsLeft)
       ld h,0
       add hl,hl
       ld e,a
       ld d,b
       ld a,d
       or a
       jr z,$F                     ;no event to process
       ex de,hl
       ld a,03h
       out (5),a
       ld ix,0C000h
       add ix,de
       ld (ix+0),l
       ld (ix+1),h
       xor a
       out (5),a
       ld ix,(NavNetMemPtr)
       inc (ix+offEventsLeft)
$$:    xor a
       ret

IsEventReceived:
;Checks to see whether a certain event and sequence ID have fired.
;Inputs:      L: sequence ID
;             H: event code
;Outputs:     Returns NC if found (and event is removed from queue)
       ld ix,(NavNetMemPtr)
       ld b,(ix+offEventsLeft)
       ld a,b
       or a
       scf
       ret z
       di
       ld ix,0C000h
findEventLoop:
       ld a,03h
       out (5),a
       ld e,(ix+0)
       ld d,(ix+1)
       xor a
       out (5),a
       di
       ld a,03h
       out (5),a
       inc ix
       inc ix
       ;HACK: we actually don't care about our sequence ID, because the device is keeping track of its own sequence ID
       ;This whole system needs to be reworked
;       push hl
;       or a
;       sbc hl,de
;       pop hl
       ld a,d
       cp h
       jr nz,eventNotFound
       ;We found it, now shift all the other events down
       dec b
       jr z,$F
       xor a
       out (5),a
       di
       push ix
       pop de
       dec de
       dec de
       ld a,03h
       out (5),a
       ld h,0
       ld l,b
       add hl,hl
       ld b,h
       ld c,l
       ld h,d
       ld l,e
       inc hl
       inc hl
       ldir
$$:    xor a
       out (5),a
       ld ix,(NavNetMemPtr)
       dec (ix+offEventsLeft)
       ret
eventNotFound:
       djnz findEventLoop
       xor a
       out (5),a
       scf
       ret