; Copyright 1996 Acorn Computers Ltd
;
; Licensed under the Apache License, Version 2.0 (the "License");
; you may not use this file except in compliance with the License.
; You may obtain a copy of the License at
;
;     http://www.apache.org/licenses/LICENSE-2.0
;
; Unless required by applicable law or agreed to in writing, software
; distributed under the License is distributed on an "AS IS" BASIS,
; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
; See the License for the specific language governing permissions and
; limitations under the License.
;
; > LegacyModes
;

        GET     s.vdu.VduModes


; In: R0 -> dest
;     R6 -> src
; Out: R2,R4,R7-R8 corrupt
;     R0, R6 pointing 8 bytes beyond control list terminator
CopyVIDCList ROUT
        ASSERT  VIDCList3BaseSize = 64
        LDMIA   r6!, {r2,r4,r7-r8}
        STMIA   r0!, {r2,r4,r7-r8}
        LDMIA   r6!, {r2,r4,r7-r8}
        STMIA   r0!, {r2,r4,r7-r8}
        LDMIA   r6!, {r2,r4,r7-r8}
        STMIA   r0!, {r2,r4,r7-r8}
        LDMIA   r6!, {r2,r4,r7-r8}
        STMIA   r0!, {r2,r4,r7-r8}
        ; It's our VIDC list, so no need to check buffer limit
81
        LDMIA   r6!, {r2,r4}
        CMP     r2, #-1
        STMIA   r0!, {r2,r4}
        BNE     %BT81
        MOV     pc, lr


; Adjust the VIDC list on the stack by the *TV parameters
; In: r7 = mode flags (for character height)
;     sp -> VIDC list
; Out: r0, r6 corrupt
ApplyTVToVIDCList
        ; Adjust vertical porch parameters
        LDROSB  r6, TVVertical
        MOV     r6, r6, LSL #24          ; sign extend to 32 bits
        MOV     r6, r6, ASR #24-3        ; and multiply by 8
        TST     r7, #ModeFlag_GapMode    ; gap mode ?
        ADDNE   r6, r6, r6, ASR #2       ; add on 2 rows if so
        TST     r7, #ModeFlag_DoubleVertical ; if double vertical
        ADDNE   r6, r6, r6               ; then double it
        LDR     r0, [sp, #VIDCList3_VertiBackPorch]
        SUBS    r0, r0, r6
        MOVMI   r0, #0
        STR     r0, [sp, #VIDCList3_VertiBackPorch]   ;subtract from back porch, clamp at 0
        LDR     r0, [sp, #VIDCList3_VertiFrontPorch]
        ADDS    r0, r0, r6
        MOVMI   r0, #0
        STR     r0, [sp, #VIDCList3_VertiFrontPorch]  ;add to front porch, clamp at 0
        ; Apply interlace setting
        LDROSB  r6, TVInterlace
        TST     r6, #1
        LDR     r0, [sp, #VIDCList3_SyncPol]
        ORRNE   r0, r0, #SyncPol_InterlaceSpecified                     ; specify as non-interlaced
        ORREQ   r0, r0, #(SyncPol_InterlaceSpecified+SyncPol_Interlace) ; specify as interlaced
        STR     r0, [sp, #VIDCList3_SyncPol]
        MOV     pc, lr


; In: R1 = Service_ModeExtension
;     R2 = Mode number (shadow bit clear) / selector block
;     R3 = Monitor type, -1 for don't care
;     R4 = Memory bandwidth available (ignore if R3=-1) (legacy, responsibility of GraphicsV)
;     R5 = VRAM available (ignore if R3=-1) (legacy, responsibility of GraphicsV)
; Out: R1 = 0 to claim service
;      R2 preserved
;      R3 -> VIDC list
;      R4 -> mode workspace list
;
; GraphicsV mode vetting should only be performed if R3 <> -1 (also means the
; returned VIDC list is technically useless - but we still need to return one)
;
; No mode substitution should be performed (that's the job of ModeTranslation)
; (Not even allowed to substitute for equivalent mode selector blocks?)
;
; Note: Returned mode workspace list will be empty (only base mode specified).
; This is because, for all Service_ModeExtension claimants, the kernel requires
; the base mode to be a member of the builtin mode list - i.e. it won't attempt
; to perform iterative Service_ModeExtension calls if the base mode is one
; which is unknown to the kernel.
;
; If ExtraBytes are needed, are we allowed to communicate that via the mode
; workspace list? (Is anything capable of using a result returned like that?
; ScreenModes doesn't deal with numbered modes, which means OS_ReadModeVariable
; won't be able to reflect any driver-mandated LineLength changes, since the
; service call can only return mode workspace lists for numbered modes; so is
; probably safe/best to not modify workspace in response to ExtraBytes)
;
HandleServiceModeExtension ROUT
        ; Check monitor type is recognised
        CMP     r3, #-1 ; Never HI
        CMPNE   r3, #NumMonitorTypes-1
        MOVHI   pc, lr
        Entry   "r0,r2,r4,r6-r8", VIDCList3Size
        CMP     r2, #NumModes
        BLO     %FT30
        CMP     r2, #256
        EXIT    LO ; Unknown numbered mode
        TST     r2, #3
        EXIT    NE ; Looks like a sprite mode word
        ; Try finding a VIDC list for a supported mode which has the right width & height (& framerate)
        ASSERT  ModeSelector_Flags = 0
        ASSERT  ModeSelector_XRes = 4
        ASSERT  ModeSelector_YRes = 8
        ASSERT  ModeSelector_PixelDepth = 12 ; (ignored at this stage)
        ASSERT  ModeSelector_FrameRate = 16
        LDMIA   r2, {r0,r2,r4,r6,r7}
        CMP     r0, #ModeSelectorFlags_ValidFormat
        EXIT    NE ; Invalid mode selector block
        ; Default to monitor type 1 if "don't care" type (don't bother trying to deal with mode 23)
        CMP     r3, #-1
        MOV     r6, #NumModes
        MULNE   r6, r3, r6
        ADRL    r8, BigVIDCTable
        ADD     r6, r8, r6, LSL #2
        MOV     r0, #NumModes-1
10
        LDR     lr, [r6, r0, LSL #2]
        CMP     lr, #-1
        BEQ     %FT20
        ADD     lr, lr, r8 ; -> VIDC list
        LDR     r1, [lr, #VIDCList3_HorizDisplaySize]
        CMP     r1, r2
        BNE     %FT20
        ; Adjust vertical size when two distinct fields
        LDR     r1, [lr, #VIDCList3_SyncPol]
        TST     r1, #SyncPol_InterlaceFields
        LDR     r1, [lr, #VIDCList3_VertiDisplaySize]
        MOVNE   r1, r1, LSL #1
        CMP     r1, r4
        BNE     %FT20
        ; XRes, YRes check out. Now check framerate
        CMP     r7, #-1
        LDRNE   r1, [lr, #-4] ; Try the frame rate from the VIDC list first
        CMPNE   r1, r7
        ADRNEL  r1, FrameRateTable ; HACK: Try FrameRateTable second (to cope with other bits which use FrameRateTable)
        LDRNEB  r1, [r1, r0]
        CMPNE   r1, r7
        ; Found a match!
        MOVEQ   r1, #Service_ModeExtension
        MOVEQ   r6, lr
        BEQ     %FT50
20
        ; Try next mode
        SUBS    r0, r0, #1
        BGE     %BT10
        MOV     r1, #Service_ModeExtension
        EXIT

30
        ; Mode number provided
        MOV     r6, r3
        CMP     r3, #-1
        BNE     %FT40
        ; Pick a monitor type so we can return a VIDC list (does the API really require this?)
        CMP     r2, #23
        MOVNE   r6, #1 ; type 1 has all modes except 23
        MOVEQ   r6, #2 ; type 2 only has 23
40
        MOV     lr, #NumModes
        MLA     r6, lr, r6, r2
        ADRL    lr, BigVIDCTable
        LDR     r6, [lr, r6, LSL #2]
        CMP     r6, #-1
        EXIT    EQ
        ADD     r6, r6, lr ; -> VIDC list

50
        ; Make copy of VIDC list on stack
        MOV     r0, sp
        BL      CopyVIDCList
        MOV     r6, sp

        ; Get the mode workspace + flags
        FRAMLDR r2
        CMP     r2, #256
        BHI     %FT60
        ADRL    lr, Vwstab
        LDR     r4, [lr, r2, LSL #2]
        ADD     r4, r4, lr ; -> mode variables
        LDR     r7, [r4, #wkModeFlags]
        ADD     r4, r4, #wksize ; -> mode workspace list
        B       %FT79

60
        ; Search for ModeFlags, NColour, LineLength in the variable list
        Push    "r9-r10"
        LDR     r8, [r2, #ModeSelector_PixelDepth]
        LDR     r9, [r2, #ModeSelector_XRes]
        MOV     r9, r9, LSL r8
        ADD     r9, r9, #7
        MOV     r10, r9, LSR #3 ; Default LineLength
        ADD     r6, r2, #ModeSelector_ModeVars
        MOV     lr, #1
        CMP     r8, #3
        MOV     r2, lr, LSL r8 ; Log2BPP -> BPP
        RSB     r2, lr, lr, LSL r2 ; BPP -> NColour
        MOVEQ   r7, #ModeFlag_FullPalette
        MOVNE   r7, #0
65
        LDR     lr, [r6], #8
        CMP     lr, #-1
        BEQ     %FT70
        CMP     lr, #VduExt_ModeFlags
        LDREQ   r7, [r6, #-4]
        CMP     lr, #VduExt_NColour
        LDREQ   r2, [r6, #-4]
        CMP     lr, #VduExt_LineLength
        LDREQ   r10, [r6, #-4]
        B       %BT65
70
        ; Patch correct colour depth into VIDC list
        ; Assuming that the VIDC list won't already specify ModeFlags or NColour control list items
        STR     r8, [sp, #8 + VIDCList3_PixelDepth] ; Patch in correct Log2BPP
        ; Deal with curious NColour value for BBC gap modes
        TST     r7, #ModeFlag_BBCGapMode
        BEQ     %FT75
        CMP     r8, #1
        CMPEQ   r2, #1
        MOVEQ   r2, #3
75
        ; Mask out unwanted mode flags
        BIC     lr, r7, #ModeFlag_NonGraphic :OR: ModeFlag_Teletext :OR: ModeFlag_GapMode :OR: ModeFlag_BBCGapMode :OR: ModeFlag_HiResMono :OR: ModeFlag_DoubleVertical :OR: ModeFlag_HardScrollDisabled
        BIC     lr, lr, #ModeFlag_InterlacedMode
        ; Massage flags for RGB modes a bit
        ASSERT  ModeFlag_DataFormatFamily_RGB = 0
        TST     lr, #ModeFlag_DataFormatFamily_Mask
        ; Clear the greyscale flag
        BICEQ   lr, lr, #ModeFlag_GreyscalePalette
        ; Detect 64 colour modes and convert to 256 colour
        CMPEQ   r2, #63
        MOVEQ   r8, #3
        MOVEQ   r2, #255
        ORREQ   lr, lr, #ModeFlag_FullPalette
        MOV     r4, #ControlList_NColour
        STR     r4, [r0, #-8]
        STR     r2, [r0, #-4] ; NColour
        MOV     r2, #ControlList_ModeFlags
        STMIA   r0, {r2,lr} ; ModeFlags

        ; The OS requires that framebuffer rows start on word boundaries;
        ; if padding is needed for this, signal it to the driver via ExtraBytes,
        ; so that drivers don't need to know of the OS's limitation (although
        ; we do assume that the driver doesn't request that ExtraBytes be
        ; modified to something inappropriate!)
        ADD     r10, r10, #3
        BIC     r10, r10, #3

        ; Check for custom LineLength and convert to ExtraBytes
        SUBS    lr, r10, r9, LSR #3
        Pull    "r9-r10"
        MOVGT   r2, #ControlList_ExtraBytes
        STMGTIA r0, {r2,lr}

        MOV     r8, #-1
        STR     r8, [r0, #8] ; New terminator
        MOV     r4, #0 ; No workspace list

79
        ; Arrive here with:
        ; r3 = monitor type
        ; r4 = mode workspace list
        ; r7 = requested ModeFlags
        ; sp -> VIDC list

        BL      ApplyTVToVIDCList

        MOV     r6, sp
        ; Vet with GraphicsV, if monitor type provided
        CMP     r3, #-1
        VDWS    WsPtr
        BEQ     %FT90
        Push    "r1,r3-r5"
        MOV     r0, r6
        BL      DoBasicVetMode
        Pull    "r1,r3-r5",EQ
        EXIT    EQ
        ; Add/update the ExtraBytes control list item
        TST     r0, #GVVetMode2_ExtraBytes_Invalid
        BEQ     %FT85
        ADD     r0, r6, #VIDCList3_ControlList
80
        LDR     r1, [r0], #8
        CMP     r1, #ControlList_Terminator
        STREQ   r1, [r0]
        MOVEQ   r1, #ControlList_ExtraBytes
        STREQ   r1, [r0, #-8]
        CMPNE   r1, #ControlList_ExtraBytes
        STREQ   r2, [r0, #-4]
        BNE     %BT80
85
        Pull    "r1,r3-r5"

90
        ; Mode is good - copy VIDC list to TempVIDCList so we can "safely" return it
        FRAMSTR r4
        ADD     r0, WsPtr, #TempVIDCList
        BL      CopyVIDCList
        ADD     r3, WsPtr, #TempVIDCList
        MOV     r1, #0
        EXIT ; claim service


; List of pixel formats supported by the kernel, in priority order for higher BPP search by HandleServiceModeTranslation
        ASSERT  GVPixelFormat_NColour = 0
        ASSERT  GVPixelFormat_ModeFlags = 4
        ASSERT  GVPixelFormat_Log2BPP = 8
        ASSERT  GVPixelFormat_Size = 12
PixelFormats ;  NColour ModeFlags             Log2BPP
        DCD     1,      0,                    0
        DCD     3,      0,                    1
        DCD     15,     0,                    2
        DCD     255,    ModeFlag_FullPalette, 3
        ; Put RISC OS 3.5 32K & 16M ahead of others in terms of priority, because they'll have the most compatibility
        DCD     65535,  0,                    4
        DCD     -1,     0,                    5
NewPixelFormats
        ; RISC OS 6 64K
        DCD     65535,  ModeFlag_64k,         4
        ; Red/blue swapped
        DCD     65535,  ModeFlag_DataFormatSub_RGB,              4
        DCD     -1,     ModeFlag_DataFormatSub_RGB,              5
        DCD     65535,  ModeFlag_DataFormatSub_RGB+ModeFlag_64k, 4
        ; 4K
        DCD     4095,   0,                                       4
        DCD     4095,   ModeFlag_DataFormatSub_RGB,              4
        ; Alpha
        DCD     65535,  ModeFlag_DataFormatSub_Alpha,                                         4
        DCD     -1,     ModeFlag_DataFormatSub_Alpha,                                         5
        DCD     65535,  ModeFlag_DataFormatSub_Alpha+ModeFlag_64k,                            4
        DCD     65535,  ModeFlag_DataFormatSub_Alpha+ModeFlag_DataFormatSub_RGB,              4
        DCD     -1,     ModeFlag_DataFormatSub_Alpha+ModeFlag_DataFormatSub_RGB,              5
        DCD     65535,  ModeFlag_DataFormatSub_Alpha+ModeFlag_DataFormatSub_RGB+ModeFlag_64k, 4
        DCD     4095,   ModeFlag_DataFormatSub_Alpha,                                         4
        DCD     4095,   ModeFlag_DataFormatSub_Alpha+ModeFlag_DataFormatSub_RGB,              4
PixelFormats_End

PixelFormats_Count * (PixelFormats_End-PixelFormats)/12
        ASSERT  PixelFormats_Count < 32


; Return in R4 a mask of which pixel formats are supported by the driver (from PixelFormats table)
GetSupportedPixelFormats ROUT
        Entry   "r0-r1,r3,r5-r12"
        ; Work out which pixel formats are supported by the driver
        VDWS    WsPtr
        LDR     r4, [WsPtr, #CurrentGraphicsVDriver]
        MOV     r4, r4, LSL #24
        ORR     r4, r4, #GraphicsV_PixelFormats
        BL      CallGraphicsV
        CMP     r4, #0
        MOVNE   r4, #2_111111 ; Old driver, just spam it with some basic formats
        EXIT    NE
        ; Scan the list to build up a flag word of supported formats
        ADR     r8, PixelFormats
        MOV     r9, #1
10
        SUBS    r1, r1, #1
        EXIT    LT
        LDMIA   r0!, {r3,r5,r6}
        ASSERT  ModeFlag_DataFormatFamily_RGB = 0
        TST     r5, #ModeFlag_DataFormatFamily_Mask
        BNE     %BT10
        MOV     r7, #0 ; Number of entries tried for {r3,r5,r6}
        ; Note that for each step of the outer loop, we retain the r8,r9
        ; values from the inner loop so that drivers which arrange their
        ; format lists in a similar order to ours will require fewer iterations
20
        LDMIA   r8!, {r10-r11,r14}
        CMP     r10, r3
        CMPEQ   r11, r5
        CMPEQ   r14, r6
        ORREQ   r4, r4, r9
        MOV     r9, r9, LSL #1
        ADDNE   r7, r7, #1
        MOVEQ   r7, #PixelFormats_Count
        CMP     r9, #1 << PixelFormats_Count
        ADREQ   r8, PixelFormats
        MOVEQ   r9, #1
        CMP     r7, #PixelFormats_Count
        BNE     %BT20
        B       %BT10


; W = Wide (line doubling expected)
; S = Square
; T = Tall (pixel doubling expected)

; Width   320  360  640  768  800  896 1024 1056 1152
; Height
;    200             W
;    240   S
;    250   S         W                   W
;    256   S         W                   W    W
;    288                  W
;    352             W              W
;    480   T    T    S
;    500             S
;    512             S
;    600                       S
;    896                                           S


; In:  R2 = Known mode number
;      R6 -> Vwstab entry
; Out: Mode selector block constructed at SP (requires MoodeSelector_ModeVars+4 space)
;      R1, R7 corrupt
;
; Note: Does not specify mode flags or eigen values. Thus the returned block
; will only represent the base, ordinary bitmap, version of the mode.
;
ModeNumberToModeSelector ROUT
        ADRL    r7, FrameRateTable
        LDRB    r7, [r7, r2]

; In:  R7 = frame rate to use
ModeNumberToModeSelector_WithFrameRate
        STR     r7, [sp, #ModeSelector_FrameRate]

        MOV     r7, #ModeSelectorFlags_ValidFormat
        STR     r7, [sp, #ModeSelector_Flags]

        LDR     r1, [r6, #wkLog2BPP]
        STR     r1, [sp, #ModeSelector_PixelDepth]     ; pixdepth = log2bpp

        LDR     r7, [r6, #wkLog2BPC]
        SUB     r1, r7, r1                             ; r1 = log2bpc-log2bpp

        LDR     r7, [r6, #wkXWindLimit]
        ADD     r7, r7, #1
        MOV     r7, r7, LSL r1
        STR     r7, [sp, #ModeSelector_XRes]

        LDR     r7, [r6, #wkYWindLimit]
        ADD     r7, r7, #1
        STR     r7, [sp, #ModeSelector_YRes]

        MOV     r7, #-1
        STR     r7, [sp, #ModeSelector_ModeVars]
        MOV     pc, lr


; In:  R2 -> Mode selector block
;      R3 = Monitor type the mode must be valid for
; Out: R2 = Corresponding mode number, else preserved
;
; Look for an exact match in all the variables
; Ignores framerate
ModeSelectorToModeNumber ROUT
        Entry   "r0-r9", wkwordsize
        MOV     r9, sp
        ; Get GenerateModeSelectorVars to do the grunt work of converting the
        ; selector into something we can easily compare against Vwstab
        BL      GenerateModeSelectorVars
        EXIT    VS ; Mode selector invalid somehow
        ADRL    r8, BigVIDCTable
        MOV     r6, #NumModes*4
        MLA     r8, r6, r3, r8 ; -> BigVIDCTable entries
        MOV     r2, #0 ; Candidate mode number
        ADRL    r1, Vwstab
10
        LDR     lr, [r8], #4
        CMP     lr, #-1
        BEQ     %FT60
        LDR     r0, [r1, r2, LSL #2]
        ADD     r0, r0, r1 ; -> mode variables
        ; Ignore YShftFactor
        LDR     r4, [r0, #wkYShftFactor]
        STR     r4, [r9, #wkYShftFactor]
        ; Simple memcmp loop
        MOV     r3, #wksize :AND: :NOT: 7
20
        LDMIA   r0!, {r4-r5}
        LDMIA   r9!, {r6-r7}
        CMP     r4, r6
        CMPEQ   r5, r7
        BNE     %FT40
        SUBS    r3, r3, #8
        BNE     %BT20
      [ wksize :AND: 4 = 4
        LDR     r4, [r0]
        LDR     r6, [r9]
        CMP     r4, r6
        BNE     %FT40
      ]
        ASSERT  wksize :AND: 3 = 0
        ; Found a match!
        FRAMSTR r2
        EXIT
40
        ; No match
        MOV     r9, sp
60
        ; Mode not supported
        ADD     r2, r2, #1
        CMP     r2, #NumModes
        BLO     %BT10
        EXIT


; In: R1 = Service_ModeTranslation
;     R2 = Mode number (shadow bit clear)
;     R3 = Monitor type (never -1)
; Out: R1 = 0 to claim service
;      R2 = Substitute mode (number or selector block)
;      R3 preserved
;
; Starting with the mode parameters from Vwstab, find an alternative
; mode which is supported by the hardware:
;
; * Try higher BPPs (including true colour)
; * Try lower BPPs
; * Try alternate resolutions
;
; Check for validity using Service_ModeExtension (although since we're only
; dealing with known modes for known monitor types, we could optimise it to
; go straight to HandleServiceModeExtension instead)
;
; Be careful with special mode types (double-vertical, BBC gap mode, etc.)
; because Service_ModeExtension won't be aware of kernel restrictions (and
; using OS_CheckModeValid instead will introduce the risk of recursion
; - although maybe we can mitigate that by only passing mode selector blocks
; to OS_CheckModeValid?)
;
; Non-teletext special mode type handling:
;
; * Allow special mode types to be downgraded to non-special types:
;   * Double-pixel to normal
;   * BBC gap mode to regular gap mode
;   * Double-vertical to normal
;   * Hi-res mono to regular mono (always downgraded)
; * There's no need to downgrade gap modes to non-gap modes, since gap modes
;   can be supported in any BPP
; * Double-pixel is the only one that will affect Service_ModeExtension, since
;   that affects the physical resolution of the mode. So we'll potentially have
;   to vet double-pixel modes twice (once with it enabled, once with it
;   disabled)
; * Theoretically we could upgrade regular modes to double-pixel in order to
;   work around hardware restrictions - but since double-pixel modes aren't
;   widely supported it's probably best to avoid that.
;
HandleServiceModeTranslation ROUT
        ; We only care about known mode numbers and monitor types
        CMP     r2, #NumModes
        CMPLO   r3, #NumMonitorTypes
        MOVHS   pc, lr
        CMP     r3, #7 ; No modes for type 7
        CMPNE   r3, #6 ; No modes for type 6
        CMPNE   r3, #2 ; Type 2 is awkward
        MOVEQ   pc, lr
        Entry   "r0,r3-r11", ModeSelector_ModeVars + 8*8 + 4 ; ModeFlags, NColour, XEig, YEig, Log2BPC, ScrRCol, ScrBRow, XWindLimit

        ; Convert to mode selector block
        ADRL    lr, Vwstab
        LDR     r6, [lr, r2, LSL #2]
        ADD     r6, r6, lr ; -> mode variables
        MOV     r7, #-1 ; We don't really care about framerate (not yet, anyway)
        BL      ModeNumberToModeSelector_WithFrameRate

        ; Fill in default ModeFlags, NColour, as if this is a simple bitmap mode
        LDR     r0, [sp, #ModeSelector_PixelDepth]
        CMP     r0, #3
        MOV     r1, #VduExt_ModeFlags
        MOVEQ   r3, #ModeFlag_FullPalette
        MOVNE   r3, #0
        MOV     r4, #VduExt_NColour
        MOV     r5, #1
        MOV     r0, r5, LSL r0 ; Log2BPP -> BPP
        RSB     r5, r5, r5, LSL r0 ; BPP -> NColour
        ADD     r0, sp, #ModeSelector_ModeVars
        MOV     r7, #-1
        STMIA   r0, {r1,r3,r4,r5,r7}

        BL      GetSupportedPixelFormats ; -> R4 = mask of supported formats

        ; Restrict pixel formats for teletext modes
        ADRL    lr, Vwstab
        LDR     r6, [lr, r2, LSL #2]
        ADD     r6, r6, lr ; -> mode variables
        LDR     r7, [r6, #wkModeFlags]
        TST     r7, #ModeFlag_Teletext
        BICNE   r4, r4, #&7

        ; Work out whether the input mode was a double-pixel mode
        LDR     r7, [r6, #wkLog2BPP]
        LDR     r6, [r6, #wkLog2BPC]
        SUBS    r7, r6, r7 ; =1 if double-pixel

        MOV     r9, r2
        FRAMLDR r3
        MOV     r2, sp

        ; Now the code to search for a substitute
        ; r0, r1 = free
        ; r2 -> candidate mode selector block
        ; r3 = monitor type
        ; r4 = mask of supported pixel formats
        ; r5, r6 = free
        ; r7 = double-pixel flag
        ; r8 = free
        ; r9 = original mode number
        ; r10, r11 = free

        ; Go straight to trying another resolution if we know the mode isn't
        ; supported by the monitor type
        MOV     lr, #NumModes
        MLA     r6, lr, r3, r9
        ADRL    lr, BigVIDCTable
        LDR     r6, [lr, r6, LSL #2]
        CMP     r6, #-1
        LDR     r10, [sp, #ModeSelector_PixelDepth]
        BEQ     %FT45

        ; Since this is a numbered mode which is known by the kernel, the
        ; kernel will have already tried the equivalent mode selector (in
        ; OfferModeExtension). But it isn't smart enough to try the non-double
        ; pixel version of the mode, so if it's a double pixel mode we'll try
        ; it again here.
        CMP     r7, #0
        BEQ     %FT31
30
        BL      OfferDoublePixelModeExtension
        BEQ     %FT50

31
        ; Try higher colour depths
        ADD     r5, r10, #1 ; Initial table index to start search
        ADR     r8, PixelFormats
        ADD     r8, r8, r5, LSL #3
        ADD     r8, r8, r5, LSL #2
        MOV     r6, #1
        MOV     r5, r6, LSL r5
32
        TST     r4, r5
        BEQ     %FT35
        LDMIA   r8, {r0,r1,lr}
        STR     r0, [sp, #ModeSelector_ModeVars+12]
        STR     r1, [sp, #ModeSelector_ModeVars+4]
        STR     lr, [sp, #ModeSelector_PixelDepth]
        BL      OfferDoublePixelModeExtension
        BEQ     %FT50 ; Success
35
        ADD     r8, r8, #GVPixelFormat_Size
        MOV     r5, r5, LSL #1
        CMP     r5, r4
        BLS     %BT32

        ; Try lower colour depths
        ADR     r8, PixelFormats
        ADD     r8, r8, r10, LSL #3
        ADD     r8, r8, r10, LSL #2
        MOV     r6, #1
        MOV     r5, r6, LSL r10
40
        MOVS    r5, r5, LSR #1
        BCS     %FT45
        SUB     r8, r8, #GVPixelFormat_Size
        TST     r4, r5
        BEQ     %BT40
        LDMIA   r8, {r0,r1,lr}
        STR     r0, [sp, #ModeSelector_ModeVars+12]
        STR     r1, [sp, #ModeSelector_ModeVars+4]
        STR     lr, [sp, #ModeSelector_PixelDepth]
        BL      OfferDoublePixelModeExtension
        BEQ     %FT50 ; Success
        B       %BT40

45
        ; Failed to find a mode
        ; Try the standard resolution for the monitor type
        MOV     r1, #480 ; VGA
        CMP     r3, #0
        MOVEQ   r1, #256 ; PAL TV
        CMP     r3, #8
        MOVEQ   r1, #200 ; NTSC TV
        ASSERT  ModeSelector_XRes = 4
        ASSERT  ModeSelector_YRes = 8
        ASSERT  ModeSelector_PixelDepth = 12
        LDMIB   sp, {r0, lr}
        CMP     r0, #640
        CMPEQ   lr, r1
        BEQ     %FT47
        MOV     r0, #640
        STMIB   sp, {r0, r1, r10} ; Write back new resolution, original depth
        ; Fill in correct ModeFlags, NColour as well
        ADR     r8, PixelFormats
        ADD     r8, r8, r10, LSL #3
        ADD     r8, r8, r10, LSL #2
        LDMIA   r8, {r0,r1}
        STR     r0, [sp, #ModeSelector_ModeVars+12]
        STR     r1, [sp, #ModeSelector_ModeVars+4]
        B       %BT30 ; Try this mode, and the non-double pixel version
47
        ; If there aren't any modes available at the standard resolution, there
        ; must be some major misconfiguration in the system. So there's not
        ; much point in doing a more exhaustive search.
        MOV     r1, #Service_ModeTranslation
        MOV     r2, r9 ; Preserved
        EXIT

50

        ; Successfully found a mode.
        ; Fill in any extra attributes that are necessary for the mode
        ADRL    lr, Vwstab
        LDR     r11, [lr, r9, LSL #2]
        ADD     r11, r11, lr ; -> mode variables
        ; XEig, YEig are always stored
        MOV     r0, #VduExt_XEigFactor
        LDR     r1, [r11, #wkXEigFactor]
        MOV     r4, #VduExt_YEigFactor
        LDR     r5, [r11, #wkYEigFactor]
        ADD     r10, sp, #ModeSelector_ModeVars + 2*8
        STMIA   r10!, {r0,r1,r4,r5}
        ; Log2BPC, XWindLimit, ScrRCol are only needed for double-pixel
        CMP     r7, #0
        LDR     r8, [sp, #ModeSelector_PixelDepth]
        BEQ     %FT51
        MOV     r0, #VduExt_Log2BPC
        LDR     lr, [sp, #ModeSelector_XRes]
        MOV     r4, #VduExt_XWindLimit
        MOV     lr, lr, LSR r7
        ADD     r1, r8, #1
        SUB     r5, lr, #1
        MOV     r7, lr, LSR #3
        MOV     r6, #VduExt_ScrRCol
        SUB     r7, r7, #1
        STMIA   r10!, {r0,r1,r4,r5,r6,r7}
51
        ; ScrBRow needed for gap modes, double-vertical
        LDR     r1, [r11, #wkModeFlags] ; Get original mode flags
        AND     r1, r1, #ModeFlag_GapMode+ModeFlag_BBCGapMode+ModeFlag_DoubleVertical+ModeFlag_NonGraphic+ModeFlag_Teletext
        LDR     r0, [sp, #ModeSelector_ModeVars+4]
        ; Teletext doesn't need any more attributes specified, and doesn't want the following rules applying
        TST     r1, #ModeFlag_Teletext
        ORR     r0, r0, r1 ; Add in the extra flags
        BNE     %FT55
        ; (non-Teletext) Double-vertical only supported by kernel for 1bpp
        CMP     r8, #0
        BICNE   r0, r0, #ModeFlag_DoubleVertical
        ; BBC gap modes only supported by kernel for 2bpp
        TST     r0, #ModeFlag_BBCGapMode
        BEQ     %FT52
        CMP     r8, #1
        BICNE   r0, r0, #ModeFlag_BBCGapMode+ModeFlag_NonGraphic ; Assume BBC gap modes and teletext are the only non-graphic modes (and teletext won't get here). So if it's no longer a BBC gap mode, it's no longer non-graphic.
        STREQ   r8, [sp, #ModeSelector_ModeVars+12] ; Set the correct NColour value if it's remaining a BBC gap mode
52
        ; Downgrade to partial 8bpp palette, for consistency with numbered modes
        CMP     r8, #3
        BICEQ   r0, r0, #ModeFlag_FullPalette
        MOVEQ   lr, #63
        STREQ   lr, [sp, #ModeSelector_ModeVars+12]
        TST     r0, #ModeFlag_GapMode+ModeFlag_DoubleVertical
        BEQ     %FT55
        ; ScrBRow needed
        MOV     r1, #8
        TST     r0, #ModeFlag_GapMode
        ADDNE   r1, r1, #2
        TST     r0, #ModeFlag_DoubleVertical
        MOVNE   r1, r1, LSL #1
        LDR     lr, [sp, #ModeSelector_YRes]
        DivRem  r5, lr, r1, r4, norem
        MOV     r4, #VduExt_ScrBRow
        SUB     r5, r5, #1
        STMIA   r10!, {r4, r5}
55
        ; Write updated mode flags
        STR     r0, [sp, #ModeSelector_ModeVars+4]
        ; Write new list terminator
        MOV     r0, #-1
        STR     r0, [r10], #4

        FRAMLDR r3 ; Clobbered by earlier OfferModeExtension success

        ; See if we can map it back to a mode number. (n.b. assuming framerate
        ; of -1 acceptable for this check)
        CMP     r8, #3
        BHI     %FT60 ; Assuming our numbered modes are all <= 8bpp
        BL      ModeSelectorToModeNumber
        BICS    r1, r2, #255
        EXIT    EQ ; r1=0, r2=substitute mode

60
        ; See if we can match the framerate of the original mode
        ADRL    r8, FrameRateTable
        LDRB    r8, [r8, r9]
        STR     r8, [sp, #ModeSelector_FrameRate]
        BL      OfferModeExtension
        MOVNE   r8, #-1
        STRNE   r8, [sp, #ModeSelector_FrameRate]
        ; Copy the mode selector block to TempModeSelector so we can safely
        ; return it.
        ADD     r2, WsPtr, #TempModeSelector
        SUB     r1, r10, sp
65
        SUBS    r1, r1, #4
        LDR     r0, [sp, r1]
        STR     r0, [r2, r1]
        BNE     %BT65
        EXIT    ; r1=0, r2=substitute mode


; OfferModeExtension, but also tries mode with half the provided width if R7<>0
; If the call succeeded for the half-width version, R7 will be set to 0 on exit
OfferDoublePixelModeExtension
        CMP     r7, #0
        BEQ     OfferModeExtension
        Entry   "r0"
        ; Double-pixel only valid for <= 16bpp
        LDR     r0, [r2, #ModeSelector_PixelDepth]
        CMP     r0, #4
        BLLS    OfferModeExtension
        EXIT    EQ
        ; Halve the width and try again
        LDR     r0, [r2, #ModeSelector_XRes]
        MOV     lr, r0, LSR #1
        STR     lr, [r2, #ModeSelector_XRes]
        BL      OfferModeExtension
        MOVEQ   r7, #0
        STRNE   r0, [r2, #ModeSelector_XRes]
        EXIT


; In: R1 = Service_EnumerateScreenModes
;     R2 = Number of modes to skip
;     R3 = Monitor type
;     R4 = Memory bandwidth (ignore?)
;     R5 = VRAM size (ignore?)
;     If filling in block:
;       R6 -> block to return data
;       R7 = space remaining
;     If counting entries/size:
;       R6 = 0
; Out: Service claimed iff R6 <> 0 and we ran out of space
;      R2 reduced by number of modes we know about
;      R6 incremented to next free space
;      R7 reduced by amount of space used/needed
;
; For the given monitor type, reduce the list of supported modes down to a list
; of (X, Y, Hz) tuples. Then try all the pixel formats for each mode.
;
; We'll assume that, for each X, Y, Hz tuple, the only variation in the VIDC
; lists will be down to the BPP; i.e. we only need to try modes based around
; the first VIDC list that we find for each X, Y, Hz value.
;
HandleServiceEnumerateScreenModes ROUT
        CMP     r3, #7 ; No modes for type 7
        CMPNE   r3, #6 ; No modes for type 6
        CMPNE   r3, #2 ; Type 2 is awkward
        MOVEQ   pc, lr
        CMP     r3, #NumMonitorTypes
        MOVHS   pc, lr
        Entry   "r0-r1,r3-r5,r8-r11"
        BL      GetSupportedPixelFormats
        MOV     r0, #0 ; Number of (X,Y,Hz) modes tried so far
        ADRL    r1, BigVIDCTable
        MOV     lr, #NumModes*4
        MLA     r3, lr, r3, r1 ; -> BigVIDCTable entry
        MOV     r8, #NumModes
10
        SUBS    r8, r8, #1
        ADDLT   sp, sp, r0, LSL #2
        EXIT    LT
        LDR     r9, [r3], #4
        CMP     r9, #-1
        BEQ     %BT10
        ADD     r9, r9, r1 ; -> VIDC list
        ; Get X, Y, Hz
        ; Adjust vertical size when two distinct fields
        LDR     r10, [r9, #VIDCList3_ControlList]
        TST     r10, #SyncPol_InterlaceFields
        LDR     r10, [r9, #VIDCList3_VertiDisplaySize]
        MOVNE   r10, r10, LSL #1
        LDR     r11, [r9, #VIDCList3_HorizDisplaySize]
        ORR     r10, r10, r11, LSL #12
        LDR     r11, [r9, #-4]
        ORR     r10, r10, r11, LSL #24
        ; Check if we've seen this before
        ADD     r11, sp, r0, LSL #2
20
        CMP     r11, sp
        BEQ     %FT30
        LDR     lr, [r11, #-4]!
        CMP     lr, r10
        BNE     %BT20
        B       %BT10
30
        ; It's a new mode, add it to the list
        ADD     r0, r0, #1
        ; Now try all the supported pixel formats
        Push    "r0-r2,r4,r6-r8,r10"
        ; Mode name won't be affected by pixel format, so build it now
        ; Max mode name length is 12 chars:
        ;  4 chars X res
        ;  4 chars Y res
        ;  4 chars for " x " + NULL
        ; Conveniently, min mode name length won't be less than 9 chars, which
        ; makes Service_EnumerateScreenModes block construction nice and easy
        MOV     r0, #0 ; Avoid garbage in the padding bytes
        Push    "r0"
        SUB     sp, sp, #8
        MOV     r0, r10, LSR #12
        MOV     r1, sp
        BIC     r0, r0, #&FF000
        MOV     r2, #12
        SWI     XOS_ConvertCardinal4
        MOV     r0, #32
        MOV     lr, #'x'
        STRB    r0, [r1], #1
        STRB    lr, [r1], #1
        STRB    r0, [r1], #1
        SUB     r2, r2, #3
        MOV     r0, r10, LSL #20
        MOV     r0, r0, LSR #20
        SWI     XOS_ConvertCardinal4

HSES_PushedSize * VIDCList3Size + 8*4 + 12 ; VIDC list, 8 words for body of Service_EnumerateScreenModes block (format 1), 12 bytes for mode name

        SUB     sp, sp, #HSES_PushedSize-12
        MOV     r6, r9
        MOV     r9, r4
        MOV     r0, sp
        BL      CopyVIDCList
        MOV     r7, #0 ; Assume standard VDU font
        BL      ApplyTVToVIDCList

        ; Recover R2, R6, R7 so we can fill in the modes in the output
        LDR     r2, [sp, #HSES_PushedSize + 8]
        LDR     r6, [sp, #HSES_PushedSize + 16]
        LDR     r7, [sp, #HSES_PushedSize + 20]
        ; r0 = free
        ; r1 = free
        ; r2 = service call R2
        ; r3 = BigVIDCTable entry
        ; r4 = free
        ; r5 = free
        ; r6 -> block to return data
        ; r7 = space remaining in block
        ; r8 = free
        ; r9 = pixel formats mask
        ; r10 = free
        ; r11 = free
        ; sp -> VIDC list

        ; Add the placeholder control list items
        MOV     r0, #ControlList_NColour
        MOV     r4, #ControlList_ModeFlags
        MOV     r8, #-1
        ADD     r5, sp, #VIDCList3_ControlList
        LDR     r10, [r5]
        CMP     r10, #-1
        ADDNE   r5, r5, #8 ; Assume only one control list item
        STMIA   r5, {r0-r1, r4-r5, r8}

        ADRL    r10, PixelFormats
        VDWS    WsPtr
40
        MOVS    r9, r9, LSR #1
        ADD     r10, r10, #12
        BCS     %FT50
45
        CMP     r9, #0
        BNE     %BT40
        ; All pixel depths tried
        ADD     sp, sp, #HSES_PushedSize
        LDMIA   sp!, {r0,r1}
        ADD     sp, sp, #4 ; skip R2
        LDR     r4, [sp], #12 ; Pull r4, skip R6 & R7
        Pull    "r8" ; Pull r8, leave r10 (new X,Y,Hz list entry)
        B       %BT10

50
        LDMDB   r10, {r0, r4, r8} ; NColour, ModeFlags, Log2BPP
        STR     r0, [r5, #4]
        STR     r4, [r5, #12]
        STR     r8, [sp, #VIDCList3_PixelDepth]
        ; Vet the mode
        MOV     r0, sp
        Push    "r2,r3"
        BL      DoBasicVetMode ; R0-R5 corrupt
        Pull    "r2,r3"
        BEQ     %BT45
        ; Mode supported
        SUBS    r2, r2, #1
        BGE     %BT45 ; Skip mode in output
        ; Is format 0 or format 1 block required?
        ADRL    r11, NewPixelFormats
        CMP     r10, r11
        MOVLS   r11, #6*4+12 ; format 0
        MOVHI   r11, #8*4+12 ; format 1
        CMP     r6, #0
        SUBEQ   r7, r7, r11
        BEQ     %BT45 ; Just measuring size
        CMP     r7, r11
        BLT     %FT90
        ; Enough space is available, so copy over the data
        SUB     r7, r7, r11
        STR     r11, [r6], #4
        CMP     r11, #6*4+12
        MOVEQ   r11, #1 ; format 0
        MOVNE   r11, #3 ; format 1
        STR     r11, [r6], #4
        ; XRes, YRes follow
        LDR     lr, [sp, #HSES_PushedSize + 7*4] ; Recover X,Y,Hz value
        LDR     r4, =&FFF
        AND     r1, r4, lr, LSR #12
        AND     r4, lr, r4
        STMIA   r6!, {r1, r4}
        LDMDB   r10, {r0, r4, r8}
        STMNEIA r6!, {r0, r4} ; NColour, ModeFlags for format 1
        MOV     lr, lr, LSR #24
        STMIA   r6!, {r8, lr} ; Log2BPP, Framerate
        ADD     lr, sp, #HSES_PushedSize-12
        LDMIA   lr, {r0, r4, r8}
        STMIA   r6!, {r0, r4, r8}
        B       %BT45

90
        ; Out of space for next entry
        ; Claim the service call and exit immediately, with R2, R6, R7
        ; representing the modes that have been skipped / added (not the mode
        ; we've just failed to add)
        ADD     sp, sp, #HSES_PushedSize
        LDMIA   sp!, {r0,r1}
        ADD     sp, sp, #4 ; skip R2
        LDR     r4, [sp], #12 ; Pull r4, skip R6 & R7
        Pull    "r8" ; Pull r8
        ADD     sp, sp, r0, LSL #2 ; Pop the X,Y,Hz list
        PullEnv
        MOV     r1, #0
        ADD     r2, r2, #1 ; Undo the pre-decrement we performed
        MOV     pc, lr

; In: R0 -> VIDC list
;     WsPtr -> VDUWS
; Out: EQ: Mode not supported, R0-R5 corrupt
;      NE: Mode supported, R0-R5 = GraphicsV_VetMode2 response
DoFullVetMode ROUT
        Entry   "r6,r7"
        MOV     r7, #1 ; Generate fake VetMode2 response
10
        LDR     r6, [WsPtr, #CurrentGraphicsVDriver]
        MOV     r6, r6, LSL #24
        ORR     r4, r6, #GraphicsV_VetMode2
        MOV     r1, #0
        BL      CallGraphicsV
        TEQ     r4, #0
        BNE     %FT50
        ASSERT  GVVetMode2_Result_Unsupported = 0
        TST     r0, #GVVetMode2_ResultMask
        EXIT
50
        ; VetMode2 not supported, fall back to VetMode
        ORR     r4, r6, #GraphicsV_VetMode
        BL      CallGraphicsV
        TEQ     r4, #0
        TEQEQ   r0, #0
        BNE     %FT90
        ; Mode supported
        MOVS    r0, r7
        BEQ     %FT60
        ; Fake up the extra return values
        ORR     r4, r6, #GraphicsV_DisplayFeatures
        MOV     r0, #0
        MOV     r2, #0
        BL      CallGraphicsV
        TST     r0, #GVDisplayFeature_VariableFramestore
        MOVNE   r0, #GVVetMode2_Result_UnkFramestore
        BNE     %FT60
        TST     r0, #GVDisplayFeature_SeparateFramestore
        MOVEQ   r0, #GVVetMode2_Result_SysFramestore
        BEQ     %FT60
        ; Fixed external framestore in use - get params
        ORR     r4, r6, #GraphicsV_FramestoreAddress
        BL      CallGraphicsV
        MOV     r3, r0
        MOV     r5, r1
        MOV     r0, #GVVetMode2_Result_ExtFramestore
60
        CMP     r0, #-1 ; NE
        EXIT

90
        ; Mode not supported
        CMP     r0, r0 ; EQ
        EXIT


; In: R0 -> VIDC list
;     WsPtr -> VDUWS
; Out: R0-R5 corrupt, except:
;      EQ: Mode not supported
;      NE: Mode supported, R0, R2 = ExtraBytes info from GraphicsV_VetMode2
DoBasicVetMode
        ALTENTRY
        MOV     r7, #0 ; Don't generate fake VetMode2 response
        B       %BT10

; TODO: Service_MonitorLeadTranslation to deal with finding last-ditch fallback mode?

        END