loop:        sei
            rcall    Wstaw_Se_Odczyt_Z_Ulubionej_Klawiatury
            ldi        zl,low(Conversion*2)
            ldi        zh,high(Conversion*2)
            add        zl,r16
            adc        zh,zero
            lpm        r17,z
            cpi        r17,0x40
            brlo    standard_key
            rcall    Handle_Special_Key
            rjmp    loop_nc
standard_key:
            ser        r18
            eor        r17,r18
            andi    r17,0x3F
            andi    r16,0x80
            or        r16,r17
            sts        Char,r16
loop_nc:    rjmp    loop
;-------------------------------------------------------------------------------
Handle_Special_Key:
            cpi        r17,0x40    ; control
            breq    Handle_Special_Key_ctrl
            cpi        r17,0x41    ; shift
            breq    Handle_Special_Key_shift
            cpi        r17,0x42    ; start
            breq    Handle_Special_Key_start
            cpi        r17,0x43    ; select
            breq    Handle_Special_Key_select
            cpi        r17,0x44    ; option
            breq    Handle_Special_Key_option
            cpi        r17,0x45    ; reset
            breq    Handle_Special_Key_reset
            ret
Handle_Special_key_ctrl:
            andi    r16,0x80
            brne    Handle_Special_key_ctrl_release
            lds        r17,Flags
            ori        r17,0x01    ; set control
            sts        Flags,r17
            ret
Handle_Special_Key_ctrl_release:
            lds        r17,Flags
            andi    r17,0xFE    ; clear control
            sts        Flags,r17
            ret
Handle_Special_key_shift:
            andi    r16,0x80
            brne    Handle_Special_key_shift_release
            lds        r17,Flags
            ori        r17,0x02    ; set shift
            sts        Flags,r17
            ret
Handle_Special_Key_shift_release:
            lds        r17,Flags
            andi    r17,0xFD    ; clear shift
            sts        Flags,r17
            ret
Handle_Special_key_start:
            andi    r16,0x80
            brne    Handle_Special_key_start_release
            cbi        PORTD,KEY_START
            ret
Handle_Special_Key_start_release:
            sbi        PORTD,KEY_START
            ret
Handle_Special_key_select:
            andi    r16,0x80
            brne    Handle_Special_key_select_release
            cbi        PORTD,KEY_SELECT
            ret
Handle_Special_Key_select_release:
            sbi        PORTD,KEY_SELECT
            ret
Handle_Special_key_option:
            andi    r16,0x80
            brne    Handle_Special_key_option_release
            cbi        PORTD,KEY_OPTION
            ret
Handle_Special_Key_option_release:
            sbi        PORTD,KEY_OPTION
            ret
Handle_Special_key_reset:
            andi    r16,0x80
            brne    Handle_Special_key_reset_release
            cbi        PORTB,KEY_RESET
            mov        r16,zero
            sts        InSync,r16
            ret
Handle_Special_Key_reset_release:
            sbi        PORTB,KEY_RESET
            ret
IRQ_pokey:    push    r16
            in        r16,SREG
            push    r16
            push    r17
            push    r18
            sbi        PORTB,POKEY_KR1
            sbi        PORTB,POKEY_KR2
            lds        r16,InSync
            tst        r16
            breq    IRQ_pokey_not_synced
            lds        r16,Counter
            in        r17,PIND
            mov        r18,r17
            andi    r17,(1<<POKEY_K0)    ; PD2
            andi    r18,(1<<POKEY_K5)    ; PD3
            lsr        r17
            lsr        r17
            lsl        r18
            lsl        r18
            or        r17,r18
            andi    r16,0x21            ; bit 5 & bit 0
            cp        r16,r17
            brne    IRQ_pokey_not_synced
            lds        r16,Counter
            lds        r17,Char
            cp        r16,r17
            brne    IRQ_pokey_cont1
            cbi        PORTB,POKEY_KR1
            ser        r18
IRQ_pokey_cont1:
            cpi        r16,0x2F    ; shift line
            brne    IRQ_Pokey_cont2
            lds        r17,Flags
            andi    r17,0x02
            breq    IRQ_Pokey_cont2
            cbi        PORTB,POKEY_KR2
IRQ_pokey_cont2:
            cpi        r16,0x3F    ; control line
            brne    IRQ_Pokey_cont3
            lds        r17,Flags
            andi    r17,0x01
            breq    IRQ_Pokey_cont3
            cbi        PORTB,POKEY_KR2
IRQ_pokey_cont3:
            lds        r16,Counter
            inc        r16
            andi    r16,0x3F    ; max count value
            sts        Counter,r16
IRQ_pokey_exit:
            pop        r18
            pop        r17
            pop        r16
            out        SREG,r16
            pop        r16
            reti
IRQ_pokey_not_synced:
            cbi        PORTB,ST_LED
IRQ_pokey_not_synced_l0:
            in        r16,PIND
            andi    r16,(1<<POKEY_K5)
            breq    IRQ_pokey_not_synced_l0
IRQ_pokey_not_synced_l1:
            in        r16,PIND
            andi    r16,(1<<POKEY_K5)
            brne    IRQ_pokey_not_synced_l1
            mov        r16,one
            sts        InSync,r16
            clr        r16
            sts        Counter,r16
            sbi        PORTB,ST_LED
            rjmp    IRQ_pokey_exit
Conversion:
; control - 0x40, shifts - 0x41, start - 0x42, select - 0x43, option - 0x44, reset - 0x45, cursor mode - 0x46
;                  00   01   02   03   04   05   06   07   08   09   0A   0B   0C   0D   0E   0F
            .db 0x80,0x1C,0x1F,0x1E,0x1A,0x18,0x1D,0x1B,0x33,0x35,0x30,0x32,0x0E,0x0F,0x34,0x2C    ; 00
            .db 0x2F,0x2E,0x2A,0x28,0x2D,0x2B,0x0B,0x0D,0x08,0x0A,  00,  00,0x0C,0x40,0x3F,0x3E    ; 10
            .db 0x3A,0x38,0x3D,0x39,0x01,0x05,  00,0x02,  00,0x22,0x41,  00,0x17,0x16,0x12,0x10    ; 20
            .db 0x15,0x23,0x25,0x20,0x22,0x26,0x41,  00,  00,0x21,0x3C,0x11,0x42,0x43,0x44,0x45    ; 30
            .db 0x80,0x80,0x80,0x80,0x46,0x80,0x80,  00,0x03,0x80,0x0E,0x13,0x80,0x14,0x06,0x80    ; 40
            .db 0x04,0x80,  00,  00,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80    ; 50
            .db   00,  00,0x11,  00,  00,0x26,0x07,0x33,0x35,0x30,0x18,0x1D,0x1B,0x1F,0x1E,0x1A    ; 60
            .db 0x32,0x22,0x0C,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80    ; 70
; key released
            .db 0x80,0x1C,0x1F,0x1E,0x1A,0x18,0x1D,0x1B,0x33,0x35,0x30,0x32,0x0E,0x0F,0x34,0x2C    ; 00
            .db 0x2F,0x2E,0x2A,0x28,0x2D,0x2B,0x0B,0x0D,0x08,0x0A,  00,  00,0x0C,0x40,0x3F,0x3E    ; 10
            .db 0x3A,0x38,0x3D,0x39,0x01,0x05,  00,0x02,  00,0x22,0x41,  00,0x17,0x16,0x12,0x10    ; 20
            .db 0x15,0x23,0x25,0x20,0x22,0x26,0x41,  00,  00,0x21,0x3C,0x11,0x42,0x43,0x44,0x45    ; 30
            .db 0x80,0x80,0x80,0x80,0x46,0x80,0x80,  00,0x03,0x80,0x0E,0x13,0x80,0x14,0x06,0x80    ; 40
            .db 0x04,0x80,  00,  00,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80    ; 50
            .db   00,  00,0x11,  00,  00,0x26,0x07,0x33,0x35,0x30,0x18,0x1D,0x1B,0x1F,0x1E,0x1A    ; 60
            .db 0x32,0x22,0x0C,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80    ; 70