.org 0x0
.section .iv,"a"
interrupt_vector:
    b RESET_HANDLER
.org 0x8
    b SYSCALL_HANDLER
.org 0x18
    b IRQ_HANDLER
.org 0x100

.text

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@        RESET HANDLER       @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

RESET_HANDLER:
    @Set interrupt table base address on coprocessor 15.
    ldr r0, =interrupt_vector
    mcr p15, 0, r0, c12, c0, 0
    @ Zera a flag de estar contando alarmes. Isso eh usado para permitir o tempo correr mesmo que o programa esteja executando ou verificando alarmes
    ldr r2, =is_on_alarm_module
    mov r0,#0
    str r0, [r2]
    @ Zera o vetor de alarmes
    ldr r1, =alarms_vector
    ldr r2, =alarms_vector_end
    reset_handler_zerador:
        str r0, [r1]
        str r0, [r1, #4]
        str r0, [r1, #8]
        add r1, r1, #12
        cmp r1, r2
        beq reset_handler_zerador
SET_GPT:
    @ Basicamente, faz as instrucoes do lab 9 uma por uma na mesma ordem que ta la
    @ Constantes para os enderecos do GPT
    .set GPT_CR,                0x53FA0000
    .set GPT_PR,                0x4
    .set GPT_SR,                0x8
    .set GPT_OCR1,              0x10
    .set GPT_IR,                0xC
    .set TIME_SZ,               200
    ldr r0, =GPT_CR
    mov r1, #0x41
    str r1, [r0]
    mov r1, #0
    str r1, [r0, #GPT_PR]
    mov r1, #TIME_SZ
    str r1, [r0, #GPT_OCR1]
    mov r1, #1
    str r1, [r0, #GPT_IR]
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)
    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]
    @instrucao msr - habilita interrupcoes
    msr  CPSR, #0x13       @ SUPERVISOR mode, IRQ/FIQ enabled
SET_GPIO:
    @ Constantes para os enderecos do GPIO
    .set GPIO_DR,             0x53F84000
    .set GPIO_GDIR,           0x4
    .set GPIO_PSR,            0x8
    ldr r0, =GPIO_DR
    ldr r2, =0xFFFC003E       @configura inputs e outputs
    str r2, [r0, #GPIO_GDIR]
SET_STACK_POINTERS:
    .set SP_user,            0x77801BBB
    .set SP_svc,             0x77801EEE
    .set SP_irq,             0x77801FFE
    ldr sp, =SP_svc            @seta o sp de svc com no maximo 819 bytes
    msr CPSR, #0x12            @comeca a troca pro modo irq
    ldr sp, =SP_irq            @seta o sp do irq  com no maximo 272 bytes
    msr CPSR, #0x1f            @comeca a trocar pro modo user
    ldr sp, =SP_user        @seta o sp de user com no maximo 955 bytes
    ldr pc, =0x77802000       @pula para o codigo de usuario

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@      SYSCALL HANDLER       @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

SYSCALL_HANDLER:
    @ Salva o contexto do usuario e pega o endereco da pilha de usuario para desempilhar
    @ os parametros das syscalls. Depois troca de volta para o modo svc
    @ Determina qual syscall foi chamada
    ldr r0, =is_on_syscall
    mov r1, #1
    str r1, [r0]
    sub r7, r7, #16
    ldr r0, =syscall_list
    mov r1, #4
    mov r2, r7
    mul r7,r1,r2
    add r0, r0, r7
    bx r0
    syscall_list:
        b sys_read_sonar
        b sys_register_proximity_callback
        b sys_set_motor_speed
        b sys_set_motors_speed
        b sys_get_time
        b sys_set_time
        b sys_set_alarm
        b sys_user_to_svc_alarms

    syscall_end:
        ldr r0, =is_on_syscall
        mov r1, #0
        str r1, [r0]
        movs pc, lr

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         SYSCALL 16         @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

sys_read_sonar:
    ldr r3, =GPIO_DR           @ carrega o endereco de GPIO_DR
    ldr r2, [r3]               @ carrega o valor de GPIO_PSR em r2
    @coloca id no mux
    msr CPSR, #0xdf
    ldr r0, [sp]         @ id do sonar a ser lido
    msr CPSR, #0x13
    Mov r0,r0,lsl #2        @ alinha o mux
    and r2, r2, #0xFFFFFFC3 @zera mux
    orr r2, r2, r0          @substitui valor em mux
    str r2, [r3]
    mov r0, #0b10           @sobe o trigger
    and r2, r2, #0xFFFFFFFD @sobe o trigger
    orr r2, r2, r0          @sobe o trigger
    str r2, [r3]            @sobe o trigger
    stmfd sp!, {r0, r1, lr}
    bl read_delay
    ldmfd sp!, {r0, r1, lr}
    mov r0, #0b00           @desce o trigger
    and r2, r2, #0xFFFFFFFD @desce o trigger
    orr r2, r2, r0          @desce o trigger
    str r2, [r3]            @desce o trigger
    read_check_flag:
        ldr r2, [r3]               @ carrega o valor de GPIO_PSR em r2
        and r2, r2, #0x00000001
        cmp r2, #1
        beq read_readtime
        stmfd sp!, {r0, r1, lr}
        bl read_delay
        ldmfd sp!, {r0, r1, lr}
        b read_check_flag
    read_readtime:
        ldr r2, [r3]
        ldr r0, =0x0003FFC0
        and r2, r2, r0
        mov r2, r2, lsr #6
        mov r0, r2
        b syscall_end

    read_delay:
        mov r0, #0
        mov r1, #150
        read_delay_loop:
            add r0, r0, #1
            cmp r0, r1
            bne read_delay_loop
        mov pc, lr

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         SYSCALL 17         @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
@rewrite this
sys_register_proximity_callback:
    .set MAX_CALLBACKS, 8
    @tratando limite de sonar
    LDMFD r8!, {r1} @ recebe a distancia limite
    LDMFD r8!, {r2} @ recebe a funcao a ser chamada
      cmp r0, #0
      blt error_read_sonar2
      cmp r0, #15
      bhi error_read_sonar2
@ Para ler o sonar, armazenar os caller safe
    stmfd sp!, {r0-r3}
    Bl sys_read_sonar @ler o sonar relacionado
    Cmp r0,r1
    Blt chamar_callback @se for menor, chamar uma callback
    b fim_register
    @tratar limite de callbacks
    @@ se for necessario fazer outra callback
chamar_callback:
    ldr r3, =MAX_CALLBACKS
    stmfd sp!, {r4}
    ldr r4, [r3]
    Cmp r4, #8
    Beq error_read_sonar1
    Add r4,r4,#1
    Str r4, [r3]
    ldmfd sp!, {r4}
    Mov pc, r2 @passa o endereco da nova funcao para o pc
error_read_sonar2:
    Mov r0, #-2
    Movs pc,lr @ ta certo???
error_read_sonar1:
    Mov r0,#-1
fim_register:
    @caso em que esta longe, entao, precisa setar um alarme.

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         SYSCALL 18         @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

sys_set_motor_speed:
    ldr r3, =GPIO_DR           @ carrega o endereco de GPIO_DR
    ldr r2, [r3]               @ carrega o valor de GPIO_PSR em r2
    msr CPSR, #0xdf
    ldr r0, [sp]         @ id
    ldr r1, [sp, #4]     @ velocidade 1
    msr CPSR, #0x13
    Cmp r1,#127
    Bhi error_velocidade @se for maior que 127, precisa de 7 bits, o que eh impossivel
    Cmp r0, #0                 @ se for 0 ou 1, esta OK!
    Beq set_motor_0
    Cmp r0, #1
    Beq set_motor_1
    B error_identificador_motor @qualquer outro valor para r0, devera retornar -1
set_motor_0:
    Mov r1,r1,lsl #19         @ passa a velocidade para os da GPIO
    And r2,r2, #0xFE03FFFF    @ zera a velocidade anterior no motor 0
    Orr r1, r1, r2            @ seta a nova velocidade
    str r1,[r3]             @ transfere para o GPIO a nova velocidade
    mov r0,#0
    b syscall_end
                    @ TALVEZ precise do delay
set_motor_1:
    Mov r1,r1,lsl #26        @análogo ao do motor 0
    And r2,r2, #0x01FFFFFF
    Orr r1, r1, r2
    str r1,[r3]
    mov r0,#0
    b syscall_end
error_velocidade:
    mov r0, #-2
    b syscall_end
error_identificador_motor:
    mov r0, #-1
    b syscall_end

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         SYSCALL 19         @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

sys_set_motors_speed:
    msr CPSR, #0xdf
    ldr r0, [sp]         @ velocidade 0
    ldr r1, [sp, #4]     @ velocidade 1
    msr CPSR, #0x13
    Cmp r0,#127
    Bhi erro_velo_0
    Cmp r1,#127
    Bhi erro_velo_1
    Ldr r3, = GPIO_DR
    Ldr r2,[r3]
    Mov r0,r0,lsl #19         @ passa a velocidade para os da GPIO
    Mov r1,r1,lsl #26         @ passa a velocidade para os da GPIO
    And r2,r2, #0xFE03FFFF    @ zera a velocidade anterior no motor 0
    And r2,r2, #0x01FFFFFF
    Orr r0, r0, r2            @ seta a nova velocidade
    orr r0, r0, r1
    str r0,[r3]               @ transfere para o GPIO a nova velocidade
    mov r0,#0
    b syscall_end

    erro_velo_0:
        mov r0,#-1
        b syscall_end
    erro_velo_1:
        mov r0,#-2
        b syscall_end

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         SYSCALL 20         @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

sys_get_time:
    ldr r0, =systime
    b syscall_end

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         SYSCALL 21         @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

sys_set_time:
    msr CPSR, #0xdf
    ldr r0, [sp]         @ id
    msr CPSR, #0x13
    ldr r1, =systime
    str r0, [r1]
    b syscall_end

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         SYSCALL 22         @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

sys_set_alarm:
    .set MAX_ALARMS, 8
    .set MAX_ALARMS_MEMORY, 96          @8x12
    stmfd sp!, {lr}
    bl sys_count_alarms                 @conta quantos alarmes estao ativos no sistema e retorna em r0
    ldmfd sp!, {lr}
    cmp r1, #MAX_ALARMS
    bhi sys_set_alarm_failure1          @retorna -1 pois ja tem alarmes demais
    msr CPSR, #0xdf                     @system mode
    ldr r0, [sp]
    ldr r1, [sp, #4]
    msr CPSR, #0x13
    ldr r3, =systime
    ldr r2, [r3]
    cmp r1, r2
    bls sys_set_alarm_failure2          @retorna -2 pois o tempo eh invalido
    stmfd sp!, {lr}
    bl sys_add_alarm                    @adiciona alarme no vetor alarm_vector
    ldmfd sp!, {lr}
    b syscall_end

    sys_count_alarms:
        ldr r0, =alarms_vector
        ldr r3, =alarms_vector_end
        mov r1, #0
        sys_count_alarms_loop:
            ldr r2, [r0]
            cmp r2, #1
            addeq r1, r1, #1
            add r0, r0, #12
            cmp r0, r3
            bne sys_count_alarms_loop
        mov pc, lr

    sys_add_alarm:
        ldr r0, =alarms_vector
        sys_search_for_empty_pos:
            ldr r2, [r0]
            cmp r2, #1
            addeq r0, r0, #12
            cmp r2, #1
            beq sys_search_for_empty_pos
        mov r1, #1
        str r1, [r0]           @ativa o alarme
        msr CPSR, #0xdf                     @system mode
        ldr r1, [sp]
        ldr r2, [sp, #4]
        msr CPSR, #0x13
        str r1, [r0, #4]        @guarda o endereco da funcao a ser chamada
        str r2, [r0, #8]        @guarda o tempo
        mov pc, lr

    sys_set_alarm_failure1:
        mov r0, #-1
        b syscall_end
    sys_set_alarm_failure2:
        mov r0, #-2
        b syscall_end

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         SYSCALL 23         @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

sys_user_to_svc_alarms: @restaura o lr de usuario, empilha seu contexto, troca pra irq e restaura seu contexto, retornando para o modulo de alarmes
    msr CPSR, #0x1f     @system mode
    ldmfd sp!, {lr}
    stmfd sp!, {r0-r12}
    msr CPSR, #0x12
    ldmfd sp!, {r0-r12}
    b return_alarm

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@         IRQ HANDLER        @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

IRQ_HANDLER:
    msr CPSR, #0xDf
    stmfd sp!, {r0-r12}        @Salva contexto do usuario
    msr CPSR, #0xD2
    ldr r0, =GPT_CR            @ Para as interrupcoes
    mov r1, #1                 @ Para as interrupcoes
    str r1, [r0, #GPT_SR]      @ Para as interrupcoes
    ldr r0, =systime           @ Incrementa o tempo
    ldr r1, [r0]               @ Incrementa o tempo
    mov r2, #1
    add r1, r1, r2             @ Incrementa o tempo
    str r1, [r0]               @ Incrementa o tempo
    ldr r0, =is_on_alarm_module
    ldr r0, [r0]
    cmp r0, #0
    stmfd sp!, {r0-r3, lr}
    bleq irq_alarm_module       @ Checa alarmes
    ldmfd sp!, {r0-r3, lr}
    msr CPSR, #0xDf
    ldmfd sp!, {r0-r12}        @Retorna contexto do usuario
    msr CPSR, #0xD2
    sub lr, lr, #4
    movs pc, lr

    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    @@@@@@@@@@@@@@        ALARM MODULE        @@@@@@@@@@@@@@
    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

    irq_alarm_module:
        ldr r0, =is_on_syscall
        ldr r0, [r0]
        cmp r0, #1
        beq cancel_check
        stmfd sp!, {r4-r7}
        ldr r0, =is_on_alarm_module @ Liga o modo de checagem de alarmes
        mov r1, #1                  @ Liga o modo de checagem de alarmes
        str r1, [r0]                @ Liga o modo de checagem de alarmes
        ldr r0, =alarms_vector
        ldr r1, =alarms_vector_end
        ldr r3, =systime
        ldr r2, [r3]
        irq_alarm_module_loop:
            ldr r3, [r0]
            cmp r3, #0
            beq alarm_not_set
            ldr r3, [r0, #4]
            cmp r2, r3                      @compara alarme com tempo de sistema e executa se o alarme ja tiver passado
            stmfd sp!, {lr}
            blhi irq_store_alarm_adress      @guarda o endereco do alarme nesse rotulo se o alarme tiver tocado
            cmp r2, r3
            bhi irq_execute_alarm           @vai executar alarme, volta atraves da syscall 23
            finished_alarm:
            ldmfd sp!, {lr}
            alarm_not_set:
            add r0, r0, #12
            cmp r0, r1
            bne irq_alarm_module_loop
        ldr r0, =is_on_alarm_module @ Desliga o modo de checagem de alarmes
        mov r1, #0                  @ Desliga o modo de checagem de alarmes
        str r1, [r0]                @ Desliga o modo de checagem de alarmes
        stmfd sp!, {r4-r7}
        cancel_check:
        mov pc, lr

        irq_store_alarm_adress:
            ldr r4, =irq_alarm_adress
            ldr r5, [r0, #8]
            str r5, [r4]
            mov pc, lr

        irq_execute_alarm:
            stmfd sp!, {r0-r12}             @ salva o contexto de irq
            msr CPSR, #0xDf
            ldmfd sp!, {r0-r12}             @ retorna o contexto do usuario
            msr CPSR, #0xD0               @ comeca a trocar pro modo user
            stmfd sp!, {lr}                 @ guarda o lr de user
            ldr r0, =irq_alarm_adress
            ldr r0, [r0]
            blx r0
            mov r7, #23    @restaura o lr de usuario, empilha seu contexto, troca pra irq e restaura seu contexto, retornando para o modulo de alarmes
            svc 0x0
            return_alarm:             @rotulo para voltar da syscall 23
            mov r3, #0
            str r3, [r0]    @desliga o alarme
            b finished_alarm

.data

systime:
    .skip 4
is_on_alarm_module:
    .skip 4
is_on_syscall:
    .skip 4
irq_alarm_adress:
    .skip 4
alarms_vector:
    @byte 1-4 = ativo, byte 5-8 = tempo, byte 9-12 = endereco da funcao a ser chamada
    .skip MAX_ALARMS_MEMORY
alarms_vector_end: