.set GPTCR, 0x53FA0000
.set GPTPR, 0x53FA0004
.set GPTOCR, 0x53FA0010
.set GPTIR, 0x53FA000C
.set GPTSR, 0x53FA0008
.set GDIR, 0x53F84004
.set DR, 0x53F84000
.set PSR, 0x53F84008
.set USERCODE, 0x77812000
.set MAX_ALARMS, 8
.set MAX_CALLBACKS, 8

.set TIME_SZ, 0x100


.org 0x0
.section .iv,"a"

_start:

interrupt_vector:

    b RESET_HANDLER
.org 0x08
    b SVC_HANDLER
.org 0x18
    b IRQ_HANDLER


.org 0x100
.text

RESET_HANDLER:

    @ Zera o contador
    ldr r2, =CONTADOR
    mov r0, #0
    str r0, [r2]

    @Faz o registrador que aponta para a tabela de interrupções apontar para a tabela interrupt_vector
    ldr r0, =interrupt_vector
    mcr p15, 0, r0, c12, c0, 0

    @Inicializa vetor GDIR conforme tabela entrada/saída

    ldr r1, =GDIR
    ldr r0, =0xFFFC003E  @ Máscara de entrada/saída
    str r0, [r1]



    @Configura o GPT

    ldr r0, =GPTCR
    mov r1, #0x41
    str r1, [r0]

    ldr r0, =GPTPR
    mov r1, #0x0
    str r1, [r0]

    ldr r0, =GPTOCR
    ldr r1, =TIME_SZ
    str r1, [r0]

    ldr r0, =GPTIR
    mov r1, #0x1
    str r1, [r0]


    @CONFIGURA O TZIC


    SET_TZIC:
    @ Constantes para os enderecos do TZIC
    .set TZIC_BASE,             0x0FFFC000
    .set TZIC_INTCTRL,          0x0
    .set TZIC_INTSEC1,          0x84
    .set TZIC_ENSET1,           0x104
    .set TZIC_PRIOMASK,         0xC
    .set TZIC_PRIORITY9,        0x424

    @ Liga o controlador de interrupcoes
    @ R1 <= TZIC_BASE

    ldr r1, =TZIC_BASE

    @ Configura interrupcao 39 do GPT como nao segura
    mov r0, #(1 << 7)
    str r0, [r1, #TZIC_INTSEC1]

    @ Habilita interrupcao 39 (GPT)
    @ reg1 bit 7 (gpt)

    mov r0, #(1 << 7)
    str r0, [r1, #TZIC_ENSET1]

    @ Configure interrupt39 priority as 1
    @ reg9, byte 3

    ldr r0, [r1, #TZIC_PRIORITY9]
    bic r0, r0, #0xFF000000
    mov r2, #1
    orr r0, r0, r2, lsl #24
    str r0, [r1, #TZIC_PRIORITY9]

    @ Configure PRIOMASK as 0
    eor r0, r0, r0
    str r0, [r1, #TZIC_PRIOMASK]

    @ Habilita o controlador de interrupcoes
    mov r0, #1
    str r0, [r1, #TZIC_INTCTRL]



    @Inicializa Pilha IRQ

    msr CPSR_c, #0xD2
    ldr sp, =pilhairq


    @Inicializa Pilha do Usuário, usando o modo system para nao ficar "trancado"

    msr CPSR_c, #0x1F
    ldr sp, =pilhauser



    @Inicializa Pilha do Supervisor

    msr  CPSR_c, #0x13
    ldr sp, =pilhasup


    @Transfere para o modo usuario

    msr CPSR_c, #0x10

    @ Salta para codigo de usuario

    @ldr r0, =USERCODE
    @mov pc, r0

    mov r0, #0
    mov r1, #30
    mov r7, #18
    svc 0x0



IRQ_HANDLER:

   ldr r0, =GPTSR
   mov r1, #1
   str r1, [r0]
   ldr r0, =CONTADOR
   ldr r1, [r0]
   add r1, r1, #1
   str r1, [r0]
   sub lr, lr, #4
   movs pc, lr


SVC_HANDLER:


   push {lr}

   msr CPSR_c, #0xD3 @ Desliga interrupcoes IRQ

   cmp r7, #16
   bleq read_sonar
   cmp r7, #17
   bleq register_proximity_callbacks
   cmp r7, #18
   bleq set_motor_speed
   cmp r7, #19
   bleq set_motors_speed
   cmp r7, #20
   bleq get_time
   cmp r7, #21
   bleq set_time
   cmp r7, #22
   bleq set_alarm

   pop {lr}

   movs pc, lr



@Tratamento das chamadas de sistema


read_sonar:

    cmp r0, #15
    bgt invalid_sonar
    cmp r0, #0
    blt invalid_sonar

    push {r2, r3, r4, r5, r6, lr}

    ldr r2, =DR
    ldr r1, [r2] @ Salva conteúdo atual de DR em r1

    @Como o Trigger está sempre em zero entre as chamadas, não é preciso o primeiro delay (pelo menos na prática não foi)


    bic r1, r1, #0b111100 @Zera os bits do MUX (2 a 5)
    orr r1, r1, r0, lsl #2 @ Seta os bits de identificação do sonar (2 a 5)
    orr r1, r1, #2 @ Seta TRIGGER (bit 2)
    str r1, [r2] @ Salva bits do sonar + TRIGGER setado

    mov r3, #0
    mov r4, #2000

        delay1:
            add r3, r3, #1
            cmp r3, r4
            blt delay1

    bic r1, r1, #2 @ Zera TRIGGER
    str r1, [r2] @ Salva TRIGGER zerado

    mov r4, #0

        flag:
            ldr r1, [r2]
            and r4, r1, #1 @ Seleciona bit da flag
            cmp r4, #1 @Verifica se bit da flag está setado
            beq flag_setada

            @Delay menor entre as checagens da flag

            mov r5, #0
            mov r6, #1000

            delay2:
                add r5, r5, #1
                cmp r5, r6
                blt delay2

                b flag

        flag_setada:

        ldr r2, =0b111111111111000000 @ Máscara que seta os bits de Sonar_Data
        and r1, r1, r2
        mov r1, r1, lsr #6 @ Ajusta localização dos bits para retorno

        mov r0, r1

        pop {r2, r3, r4, r5, r6, lr}

        mov pc, lr


    invalid_sonar:
        mov r0, #-1
        mov pc, lr


register_proximity_callbacks:

    cmp r0, #0
    blt sonar_error
    cmp r0, #15
    bgt sonar_error
    push {r3}
    cmp r1, #0
    blt distance_error
    ldr r3, =0xFFF
    cmp r1, r3
    bgt distance_error

    push {r4, r5, r6}

    ldr r3, =CALLBACKS
    ldr r4, [r3]
    cmp r4, #MAX_CALLBACKS
    bgt callback_overflow


    mov pc, lr

    sonar_error:
        mov r0, #-2
        mov pc, lr

    distance_error:
        mov r0, #-1
        pop {r3}
        mov pc, lr

    callback_overflow:
        mov r0, #-1
        pop {r3, r4, r5, r6}
        mov pc, lr

set_motor_speed:

    cmp r1, #0
    blt invalid_speed
    cmp r1, #63
    bgt invalid_speed
    cmp r0, #0
    blt invalid_motor
    cmp r0, #1
    bgt invalid_motor

    push {r2, r3}

    cmp r0, #1
    beq motor_1
    cmp r0, #0
    beq motor_0


    invalid_motor:
        mov r0, #-1
        mov pc, lr

    invalid_speed:
        mov r0, #-2
        mov pc, lr

    motor_0:

        ldr r2, =DR
        ldr r3, [r2]
        bic r3, #0x1F80000 @ Máscara que zera os bits de 19 a 24
        orr r3, r1, lsl #19 @ Máscara que seta os bits de 19 a 24
        str r3,[r2]
        mov r0, #2
        pop {r2, r3}
        mov pc, lr

    motor_1:

        ldr r2, =DR
        ldr r3, [r2]
        bic r3, #0xFC000000 @ Máscara que zera os bits de 26 a 31
        orr r3, r1, lsl #26 @ Máscara que seta os bits de 26 a 31
        str r3, [r2]
        mov r0, #2
        pop {r2, r3}
        mov pc, lr


set_motors_speed:

    cmp r1, #0
    blt invalid_1
    cmp r1, #63
    bhi invalid_1
    cmp r0, #0
    blt invalid_0
    cmp r0, #63
    blt invalid_0

    b motors


    invalid_1:
        mov r0, #-2
        mov pc, lr

    invalid_0:
        mov r0, #-1
        mov pc, lr

    motors:
        push {r2, r3}

        ldr r2, =DR
        ldr r3, [r2]
        bic r3, #0x1F80000 @ Máscara que zera os bits de 19 a 24
        orr r3, r0, lsl #19 @ Máscara que seta os bits de 19 a 24
        str r3, [r2]

        ldr r2, =DR
        ldr r3, [r2]
        bic r3, #0xFC000000 @ Máscara que zera os bits de 26 a 31
        orr r3, r1, lsl #26 @ Máscara que seta os bits de 26 a 31
        str r3, [r2]

        mov r0, #0
        pop {r2, r3}
        mov pc, lr


get_time:
	push {r1}
    ldr r1, =CONTADOR
    ldr r0, [r1]
    pop {r1}
    mov pc, lr


set_time:
	push {r1}
	ldr r1, =CONTADOR
    str r0, [r1]
    pop {r1}
    mov pc, lr


set_alarm:



.data @ Seção de Dados

CONTADOR: .word 0
ALARMS: .word 0
CALLBACKS: .word 0

strcallbacks:
.skip 72

stralarms:
.skip 72



.skip 500
pilhasup:
.skip 500
pilhauser:
.skip 500
pilhairq: