; vim: sts=4 sw=8 et ai fdm=marker ft=asm

;
;                 fnordlicht firmware
;
;    for additional information please
;    see http://koeln.ccc.de/prozesse/running/fnordlicht
;
; (c) by Alexander Neumann <alexander@bumpern.de>
;    and Lars Noschinski <lars@public.noschinski.de>
;
; This program is free software; you can redistribute it and/or modify
; it under the terms of the GNU General Public License version 2 as
; published by the Free Software Foundation.
;
; This program is distributed in the hope that it will be useful,
; but WITHOUT ANY WARRANTY; without even the implied warranty of
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
; GNU General Public License for more details.
;
; You should have received a copy of the GNU General Public License
; along with this program; if not, write to the Free Software
; Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
;
; For more information on the GPL, please go to:
; http://www.gnu.org/copyleft/gpl.html



;   CONSTANTS {{{
; ==================================================================================
; Various script handlers, if 0 or above 128 (bit7 set), script execution is disabled
.equ ScriptHandlerDisabled  = 0  ; Script execution disabled
.equ ScriptHandlerMem       = 1  ; Execute scripts in RAM
.equ ScriptHandlerFlash     = 2  ; Execute scripts in Flash
.equ ScriptHandlerWait      = 3  ; Wait till a given event occurs
.equ ScriptHandlerSleep     = 4  ; Sleeps a given number of cycles

.equ ScriptSignalStop       = 5  ; if set, signals the handler to don't execute
                                 ; any further instructions in this cycle
                                 ; (5 because of the symmetric list). fnord.

; Opcodes
.equ OpcodeNop              = 0x00
.equ OpcodeFadeChannel      = 0x10
.equ OpcodeFadeChannels     = 0x20
.equ OpcodeJump             = 0x30
.equ OpcodeSetChannel       = 0x40
.equ OpcodeSleep            = 0x50
.equ OpcodeWait             = 0x60
.equ OpcodeClear            = 0x70
; }}}

.dseg

;   table: ScriptInfoTable {{{
; =============================================================================

.equ ScriptChannels  = 3            ; number of concurrent scripts
.equ ScriptStackSize = 6            ; in byte

.equ ScrRecHandler  = 0             ; byte
.equ ScrRecPos      = 1             ; word
.equ ScrRecStackOffset = 3          ; byte
.equ ScrRecFlags    = 4             ; byte
.equ ScrRecStackBase = 5            ; stack
.equ ScrRecSize     = 5+ScriptStackSize

ScriptInfoTable:
    .byte   ScrRecSize*ScriptChannels
; }}}


.cseg

;   table: OperatorJumpTable {{{
; ==================================================================================
;   Operator order must be consistend with the Opcode* constants above

OperatorJumpTable:
        rjmp do_op_nop
        rjmp do_op_fade_channel
        rjmp do_op_fade_channels
        rjmp do_op_jump
        rjmp do_op_set_channel
        rjmp do_op_sleep
        rjmp do_op_wait
        rjmp do_op_clear
; }}}

;   table: ScriptHandlerJumpTable {{{
; ==================================================================================
;   Handler order must be consistend with the ScriptHandler* constants above

ScriptHandlerJumpTable:
        ret                         ; script disabled
        rjmp handler_mem
        rjmp handler_flash
        rjmp handler_wait
        rjmp handler_sleep

; }}}

;   MACROS {{{
; ==================================================================================

;   macro: RESTORE_HANDLER {{{
; ==================================================================================
; Restores current handler from stack
;
; uses: @0, @1, @2
; @0 is not modified, but must point to the script record.
;
;                         @0          @1       @2
; call: SAVE_HANDLER ScriptRecord TmpPointer TmpReg
;
; @0, @1 must be Y or Z, Tm

.macro MACRO_RESTORE_HANDLER
        ldd @2, @0+ScrRecStackOffset    ; Load stack pointer to @2 register

        mov @1L, @2
        subi @1L,  3                 ; decrement stack pointer offset by 3
        std @0+ScrRecStackOffset, @1L   ; store new offset

        movw @1H:@1L, @0H:@0L
        adiw @1H:@1L, ScrRecStackBase    ; stack base offset
        add @1L, @2                 ; stack data offset
        clr @2
        adc @1H, @2

        MACRO_STACK_POP @1, @2       ; Restore old stack handler
        std @0+ScrRecPos+1, @2
        MACRO_STACK_POP @1, @2
        std @0+ScrRecPos, @2
        MACRO_STACK_POP @1, @2
        std @0+ScrRecHandler, @2
.endmacro

; }}}

;   macro: SAVE_HANLDER {{{
; ==================================================================================
; Saves current handler to stack
;
; uses: @0, @1, @2
; @0 is not modified, but must point to the script record.
;
;                         @0          @1       @2
; call: SAVE_HANDLER ScriptRecord TmpPointer TmpReg
;
; @0, @1 must be Y or Z, Tm

.macro MACRO_SAVE_HANDLER
        ldd @2, @0+ScrRecStackOffset    ; Load stack pointer to @1 register

        mov @1L, @2                 ; copy stack pointer offset
        adiw @1H:@1L, 3             ; increment stack pointer offset by 3
        std @0+ScrRecStackOffset, @1L  ; store new offset

        movw @1H:@1L, @0H:@0L
        adiw @1H:@1L, ScrRecStackBase    ; stack base offset
        add @1L, @2                 ; stack data offset
        clr @2
        adc @1H, @2

        ldd @2, @0+ScrRecHandler       ; Save old handler to stack
        MACRO_STACK_PUSH @1, @2
        ldd @2, @0+ScrRecPos
        MACRO_STACK_PUSH @1, @2
        ldd @2, @0+ScrRecPos+1
        MACRO_STACK_PUSH @1, @2
.endmacro

; }}}

;   macro: MACRO_SET_HANDLER {{{
; ==================================================================================
; Sets a new script handler
;
;                        @0           @1         @3:@2
; call: SET_HANDLER ScriptRecord, NewHandler, HandlerPos
;
; @0 must be Y or Z

.macro MACRO_SET_HANDLER
        std @0+ScrRecHandler, @1
        std @0+ScrRecPos, @2
        std @0+ScrRecPos+1, @3
.endmacro

; }}}

; }}}

;   function: execute_scripts {{{
; ==================================================================================
; Calls execute_script for each script.
execute_scripts:

        ldi YL, low(LedStatusTable)     ; construct script flags from led flags
        ldi YH, high(LedStatusTable)    ; flags are given as w1 to execute_script

        clr w1                          ; clear w1, this will hold the led flags
        ldi w2, PWMChannels             ; w2 is loop counter register
es_readflags_loop:
        ldd w3, Y+LedRecFlags           ; load flags
        sbrc w3, LedFlagTargetReached   ; test if target has been reached on this channel
        ori w1, (1 << (PWMChannels-1))  ; if true: set bit in w1
        andi w3, !(1<<LedFlagTargetReached) ; if false: just clear flag
        std Y+LedRecFlags, w3           ; store flags

        dec w2                          ; decrement loop counter
        breq es_execute                 ; if zero, jump away
        adiw YH:YL, LedRecSize          ; else select next led record
        lsr w1                          ; and shift w1 right.
        rjmp es_readflags_loop          ; loop

es_execute:                             ; Execute each script
        ldi YL, low(ScriptInfoTable)
        ldi YH, high(ScriptInfoTable)

        ldi w2, ScriptChannels
es_execute_loop:
        ldd w3, Y+ScrRecFlags           ; Set new flags
        or w3, w1
        std Y+ScrRecFlags, w3
        push w1
        push w2
        rcall execute_script            ; Execute script
        pop w2
        pop w1
        adiw YH:YL, ScrRecSize
        dec w2
        brne es_execute_loop

        ret;
; }}}

;   function: execute_script {{{
; =============================================================================
; Y must point to a script record in ScriptInfoTable. Executes handler for
; this script. Y is not modified.
;
; handlers must end with a "ret" instruction
; handlers find address of current script record in Y register

execute_script:
        ldd w1, Y+ScrRecHandler     ; load id of script handler
                                    ; select jump table entry

        sbrc w1, 7                  ; if bit7 is set, this handler is disabled
        ret                         ; so return, else skip

        clr w2
        ldi ZL, low(ScriptHandlerJumpTable)
        ldi ZH, high(ScriptHandlerJumpTable)
        add ZL, w1
        adc ZH, w2

        push YL
        push YH
        icall                       ; jump to handler
        pop YH
        pop YL

        ret
; }}}

;   SCRIPT HANDLERS {{{
; =============================================================================
; Script handlers are started once early in each pwm cycle. A handler
; executes all instruction, which shall be executed in the current cycle
; and returns afterwards.
;
; If multiple scripts are running, the handlers for the script channels
; are executed in a row.
;
; If a script handler executes any opcodes, it must give the current script
; record in the X registers, opcode flags in w1 and opcode parameters in w2-w3
;
; Opcodes may return flags in w1.
;
; You can use the stack to temporarily replace a script handler. To do that,
; you push ScrRecHandler, ScrRecPos and ScrRecPos+1 on the stack and put
; the new parameters in those records
; -----------------------------------------------------------------------------

;   function handler_flash {{{
; =============================================================================
;   Executes a script in flash memory.
;   ScrRecPos contains the absolute address to the next script instruction

handler_flash:
        ldd ZL, Y+ScrRecPos         ; load current script pointer (already mul by 2)
        ldd ZH, Y+ScrRecPos+1

        lpm w1, Z+                  ; load a complete instruction
        lpm w2, Z+
        lpm w3, Z+
        lpm w4, Z+

        std Y+ScrRecPos, ZL         ; save new script pointer
        std Y+ScrRecPos+1, ZH

        mov l1, w1
        andi w1, 0xf0
        swap w1                     ; extract opcode

        ldi ZL, low(OperatorJumpTable)  ; load address of opcode
        ldi ZH, high(OperatorJumpTable)
        add ZL, w1
        clr w1
        adc ZH, w1

        mov w1,l1
        andi w1, 0x0f               ; extract flags
        icall                       ; Execute opcode

        sbrs w1, ScriptSignalStop   ; Stop execution if stop flag is set
        rjmp handler_flash

        ret
; }}}

;   function handler_mem {{{
; =============================================================================
;   Executes a script in ram.
;   ScrRecPos contains the absolute address to the next script instruction

handler_mem:
        ldd ZL, Y+ScrRecPos         ; load current script pointer
        ldd ZH, Y+ScrRecPos+1

        ld w1, Z+                  ; load a complete instruction
        ld w2, Z+
        ld w3, Z+
        ld w4, Z+

        std Y+ScrRecPos, ZL         ; save new script pointer
        std Y+ScrRecPos+1, ZH

        mov l1, w1
        andi w1, 0xf0
        swap w1                     ; extract opcode

        ldi ZL, low(OperatorJumpTable)
        ldi ZH, high(OperatorJumpTable)
        add ZL, w1
        clr w1
        adc ZH, w1

        mov w1,l1
        andi w1, 0x0f               ; extract flags

        icall                       ; Execute opcode

        sbrs w1, ScriptSignalStop   ; Stop execution if stop flag is set
        rjmp handler_flash

        ret
; }}}

;   function handler_sleep {{{
; =============================================================================
;   Sleeps a number of cycles, then restores previous handler
;   ScrRecPos contains sleep cycles.

handler_sleep:

        ldd ZL, Y+ScrRecPos         ; load sleep cycles
        ldd ZH, Y+ScrRecPos+1

        clr w3                      ; test if we still have to sleep
        cp ZL,w3
        cpc ZH,w3
        breq handler_sleep_restore

                                    ; if yes:
        sbiw ZH:ZL, 1               ; decrease sleep cycles by one
        std Y+ScrRecPos, ZL         ; and save new sleep cycles
        std Y+ScrRecPos+1, ZH
        ret

handler_sleep_restore:               ; else:

        MACRO_RESTORE_HANDLER Y, Z, w1
        ret

; }}}

;   function handler_wait {{{
; =============================================================================
;   Wait till an event (like LedFlagTargetReached) occurs.
;   ScrRecPos contains bitmask of events to wait for.
handler_wait:
        ldd w1, Y+ScrRecPos         ; load wait mask
        ldd w2, Y+ScrRecFlags       ; load current event flags

        and w1, w2
        brne handler_wait_restore   ; if nonzero, match occured

        ret                         ; else return

handler_wait_restore:
        com w1                      ; invert w1 (result of w1 AND w2)
        and w2, w1                  ; delete masked bits in flag byte
        std Y+ScrRecFlags, w2       ; store new flags

        MACRO_RESTORE_HANDLER Y, Z, w1
        ret
; }}}

; }}} SCRIPT HANDLERS

;   OPCODES {{{
; =============================================================================
; Opcodes are executed by script handlers. Flags are passed in on w1,
; parameters on w2-w4.
;
; Opcodes can pass flags back on w1, so w1 should be cleared before "ret".
; -----------------------------------------------------------------------------

;   function: do_op_fade_channel {{{
; ==========================
;   we get target value at w2, skipcycles at w3 and brightstep at w3
;   the channel is in the flags, which is in w1

do_op_fade_channel:
        ldi ZL, low(LedStatusTable)     ; Load address of led status table
        ldi ZH, high(LedStatusTable)

        tst w1                          ; select led channel to modify
        breq do_op_fade_channel_dowork

do_op_fade_channel_select_record:       ; increment by LedRecSize while w1 is positive
        adiw ZH:ZL, LedRecSize
        dec w1
        brne do_op_fade_channel_select_record

do_op_fade_channel_dowork:
        std Z+LedRecTargetVal, w2       ; store new fading values
        std Z+LedRecFadeSpeedL, w3
        std Z+LedRecFadeSpeedH, w4
        clr w1
        std Z+LedRecCycleCount, w1      ; reset internal working values - do we need to reset flags also?

        ret                             ; w1 is already cleared

; }}}

;   function: do_op_fade_channels {{{
; ==========================
;   we get target values for channel 0,1,2 at w2, w3, w4

do_op_fade_channels:
        ldi ZL, low(LedStatusTable)     ; load address of led status table
        ldi ZH, high(LedStatusTable)

        clr w1

        std Z+LedRecTargetVal, w2       ; store new target values for each channel
        std Z+LedRecCycleCount, w1      ; and reset CycleCount to 0
        adiw ZH:ZL, LedRecSize
        std Z+LedRecTargetVal, w3
        std Z+LedRecCycleCount, w1
        adiw ZH:ZL, LedRecSize
        std Z+LedRecTargetVal, w3
        std Z+LedRecCycleCount, w1

        ret                             ; w1 is already cleared

; }}}

;   function: do_op_jump {{{
; ==========================
;   we get signed jump offset at w2 and current script record at X
;
;   please note that "jump 0" is more or less equivalent to a "nop" and
;   "jump -1" is an infinite loop.

do_op_jump:
        ldd w3, Y+ScrRecPos         ; load script position pointer
        ldd w4, Y+ScrRecPos+1
        clr w1
        sbrc w2, 7                  ; skip if positive (= bit7 cleared)
        ldi w1, 0xFF                ; otherwise fill w1 with 1s (2-complement-arithmetics)

        lsl w2                      ; mul by 4 (because one instruction is 4 bytes)
        lsl w2
        add w3, w2                  ; decrement position (2-complement-arithmetics)
        adc w4, w1

do_op_jump_store:
        std Y+ScrRecPos, w3         ; store new position
        std Y+ScrRecPos+1, w4

        clr w1
        ret

; }}}

;   function: do_op_nop {{{
; ==========================
;   Do nothing =)

do_op_nop:
        clr w1
        sbr w1,(1<<ScriptSignalStop); DEBUG
        ret

; }}}

;   function: do_op_set_channel {{{
; ==========================
;   we get target the value at w2,  the channel is in the flags, which is in w1

do_op_set_channel:
        ldi ZL, low(LedStatusTable)     ; Load address of led status table
        ldi ZH, high(LedStatusTable)

        tst w1                          ; select led channel to modify
        breq do_op_set_channel_dowork

do_op_set_channel_select_record:        ; increment by LedRecSize while w1 is positive
        adiw ZH:ZL, LedRecSize
        dec w1
        brne do_op_set_channel_select_record

do_op_set_channel_dowork:
        std Z+LedRecBrightness, w2      ; set target value and brightness to the same new value
        std Z+LedRecTargetVal, w2       ; so we get no fading

        clr w1
        ret

; }}}

;   function: do_op_sleep {{{
; ==========================
;   we get number of cycles to sleep at w2, w3
;
;   "sleep 0" does nothing
;   "sleep 1" sleeps for the next pwm cycle

do_op_sleep:
        clr w1
        cp w2,w1
        cpc w3,w1
        breq do_op_sleep_return        ; sleep 0

        sbr w1,(1<<ScriptSignalStop); sleep > 0: tell the current handler to not execute
                                    ; any further instructions in this cycle

        subi w2,1
        sbci w3,0

        clr w4                      ; sleep still > 0: install handler
        cp w2,w4
        cpc w3,w4
        breq do_op_sleep_return

        MACRO_SAVE_HANDLER Y, Z, w4 ; Save old handler

        ldi w4, ScriptHandlerSleep
        MACRO_SET_HANDLER Y, w4, w2, w3 ; activate sleep handler


do_op_sleep_return:
        ret
; }}}

;   function: do_op_wait {{{
; ==========================
;   we get the event(s) to wait for at w2
;
;

do_op_wait:
        ldd w3, Y+ScrRecFlags       ; load flags
        mov w1, w3                  ; copy flags
        and w1, w2                  ; check for match
        brne do_op_wait_direct_match ; if match, jump away

        MACRO_SAVE_HANDLER Y, Z, w1

        ldi w1, ScriptHandlerWait   ; load handler offset
        clr w3                      ; empty w3
        MACRO_SET_HANDLER Y, w1, w2, w3 ; activate wait handler

        ldi w1,(1<<ScriptSignalStop) ; activate the new handler for the next pwm cycle

        rjmp do_op_wait_return

do_op_wait_direct_match:
        com w2                      ; invert mask
        and w3, w2                  ; clear bits from the mask
        std Y+ScrRecFlags, w3       ; store flags

        clr w1                      ; empty w1

do_op_wait_return:
        ret
; }}}

;   function: do_op_clear {{{
; ==========================
;   we get the event(s) to wait for at w2
;
;

do_op_clear:
        clr w1
        std Y+ScrRecFlags, w1       ; clear flags

        ret
; }}}

; }}} OPCODES