#include <AT91SAM7S128.H> /* AT91SAM7S64 definitions */
#include "..\Board.h"

volatile unsigned int ENCODER1_Position = 0; /* Текуща позиция энкодера 1*/
volatile unsigned long int ENCODER1_Count = 0; /* Текуща позиция энкодера 1*/
#define ENCODER1_A_MASK (1<<20) /* PA19 */
#define ENCODER1_B_MASK (1<<19) /* PA20 */
#define ENCODER2_A_MASK (1<<15) /* PA15 */
#define ENCODER2_B_MASK (1<<14) /* PA14 */
#define ENCODER_A_MASK (ENCODER1_A_MASK|ENCODER1_B_MASK|ENCODER2_A_MASK|ENCODER2_B_MASK)

#define ENCODER1_A (1<<20) // PA19
#define ENCODER1_B (1<<19) // PA20
#define ENCODER2_A (1<<15) // PA15
#define ENCODER2_B (1<<14) // PA14


#define ENCODER1_resolution 1024
#define ENCODER2_resolution 1024


extern AT91S_PIO * pPIO; /* Global Pointer to PIO */

extern __irq void irq0_int (void); /* IRQ0 Function */


void irq0_nint (void) { /* Nested IRQ0 (Push button SW2) */

if ((pPIO->PIO_PDSR & ENCODER1_A) == 0) { /* Проверяем активный уровень на входе А энкодера 1*/
if ((pPIO->PIO_PDSR & ENCODER1_B) == 0) { /* Определяем направление вращения энкодера 1*/
if (ENCODER1_Position == (ENCODER1_resolution-1)) {/*Проверяем не прывисл ли
ENCODER1_Position значения полосок на энкодере*/
ENCODER1_Position = 0;/*Если превысил то отсчитываем с 0*/
// ENCODER1_Count++; /*Увеличиваем на 1 значения оборотов */
}
ENCODER1_Position++;} /* Если за часовой стрелкой то увеличиваем на 1*/
else {
if (ENCODER1_Position == 0) { /* Проверяем не прывисл ли ENCODER1_Position значения 0*/
ENCODER1_Position = (ENCODER1_resolution-1);/*Если 0 то отсчитываем с ENCODER1_resolution-1*/
ENCODER1_Count++; /*Уменьшаем на 1 значения оборотов */
}
ENCODER1_Position--; } /* Если против часовой стрелкой то уменьшаем на 1*/
}

*AT91C_AIC_EOICR = 0; /* End of Interrupt*/
}


void init_extint (void) { /* Setup IRQ 0 Interrupt */
AT91S_AIC * pAIC = AT91C_BASE_AIC; /* Setup IRQ0 Interrupt Mode and Vector with Priority 0 and Enable it */
/*
AIC_SMR - Регистр источника прерывания 0
AT91C_ID_IRQ0 - ID Прерывание для IRQ0
AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE - Internal Sources Code Label Positive
Edge triggered - Положительный фронт */
pAIC->AIC_SMR[AT91C_ID_IRQ0] = AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE | 0;// AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE
/* Вектор прерывания. AIC_SVR0..AIC_SVR31 - В эти регистры записываются
адреса соответствующих процедур обработки прерывания для каждого источника.*/
pAIC->AIC_SVR[AT91C_ID_IRQ0] = (unsigned long) irq0_int; /*Регистр разрешения прерываний*/
pAIC->AIC_IECR = (1 << AT91C_ID_IRQ0);
}

#include <AT91SAM7S128.H> /* AT91SAM7S64 definitions */
#include "..\Board.h"

#define BR 115200 /* Baud Rate */

#define BRD (MCK/16/BR) /* Baud Rate Divisor */


AT91S_USART * pUSART = AT91C_BASE_US0; /* Global Pointer to USART0 */


void init_serial (void) { /* Initialize Serial Interface */

*AT91C_PIOA_PDR = AT91C_PA5_RXD0 | /* Enable RxD0 Pin */
AT91C_PA6_TXD0; /* Enalbe TxD0 Pin */

pUSART->US_CR = AT91C_US_RSTRX | /* Reset Receiver */
AT91C_US_RSTTX | /* Reset Transmitter */
AT91C_US_RXDIS | /* Receiver Disable */
AT91C_US_TXDIS; /* Transmitter Disable */

pUSART->US_MR = AT91C_US_USMODE_NORMAL | /* Normal Mode */
AT91C_US_CLKS_CLOCK | /* Clock = MCK */
AT91C_US_CHRL_8_BITS | /* 8-bit Data */
AT91C_US_PAR_NONE | /* No Parity */
AT91C_US_NBSTOP_1_BIT; /* 1 Stop Bit */

pUSART->US_BRGR = BRD; /* Baud Rate Divisor */

pUSART->US_CR = AT91C_US_RXEN | /* Receiver Enable */
AT91C_US_TXEN; /* Transmitter Enable */
}


int sendchar (int ch) { /* Write character to Serial Port */

if (ch == '\n') { /* Check for CR */
while (!(pUSART->US_CSR & AT91C_US_TXRDY)); /* Wait for Empty Tx Buffer */
pUSART->US_THR = '\r'; /* Output CR */
}
while (!(pUSART->US_CSR & AT91C_US_TXRDY)); /* Wait for Empty Tx Buffer */
return (pUSART->US_THR = ch); /* Transmit Character */
}


int getkey (void) { /* Read character from Serial Port */

while (!(pUSART->US_CSR & AT91C_US_RXRDY)); /* Wait for Full Rx Buffer */
return (pUSART->US_RHR); /* Read Character */
}

AREA IRQ0, CODE, READONLY
ARM

PRESERVE8
ALIGN
IMPORT irq0_nint
EXPORT irq0_int
irq0_int FUNCTION

SUB LR, LR, #4 ; Update Link Register
STMFD SP!, {R0-R12, LR} ; Save Workspace & LR to Stack
MRS R0, SPSR ; Copy SPSR to R0
STMFD SP!, {R0, R1} ; Save SPSR to Stack (8-byte)
MSR CPSR_c, #0x1F ; Enable IRQ (Sys Mode)
STMFD SP!, {R0, LR} ; Save LR_sys to Stack (8-byte)
LDR R0, =irq0_nint ; Get IRQ Function Address
MOV LR, PC ; Return Address
BX R0 ; Call IRQ Function
LDMFD SP!, {R0, LR} ; Restore LR_sys
MSR CPSR_c, #0x92 ; Disable IRQ (IRQ Mode)
LDMFD SP!, {R0, R1} ; Restore SPSR to R0
MSR SPSR_cxsf, R0 ; Copy R0 to SPSR
LDMFD SP!, {R0-R12, PC}^ ; Return to program

ENDFUNC

END

                STMFD   SP!, {R0-R12, LR}    ; Save Workspace & LR to Stack
                ...
                LDMFD   SP!, {R0-R12, PC}^   ; Return to program

                STMFD   SP!, {R0-R3, R12, LR}    ; Save Workspace & LR to Stack
                ...
                LDMFD   SP!, {R0-R3, R12, PC}^   ; Return to program

void pioa_irq_init(void)
{
AT91C_BASE_AIC->AIC_SVR[AT91C_ID_PIOA] = (unsigned int)irq_pioa;
AT91C_BASE_AIC->AIC_SMR[AT91C_ID_PIOA] = AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL | PIOA_IRQ_PRIORITY;
AT91C_BASE_AIC->AIC_IECR = (1UL << AT91C_ID_PIOA);
AT91C_BASE_SYS->PIOA_IER = PINX_MASK | PINY_MASK;
}

__irq void irq_pioa(void)
{
unsigned int status = AT91C_BASE_SYS->PIOA_ISR;
if(status & PINX_MASK)
{
...
}
if(status & PINY_MASK)
{
...
}
AT91C_BASE_AIC->AIC_EOICR = status;
}

#define ENCODER1_A             (1<<20) // PA19
#define ENCODER1_B             (1<<19) // PA20

void irq0_nint (void) {
if ((pPIO->PIO_PDSR & ENCODER1_B) == 0) {
        ENCODER1_Position++;}
    else {      ENCODER1_Position--; }
*AT91C_AIC_EOICR = 0;
AT91C_BASE_PIOA->PIO_ISR;}

#define    PIOA_IRQ_PRIORITY    7

                AREA    IRQ0, CODE, READONLY
                ARM
                PRESERVE8
                ALIGN
                IMPORT  irq0_nint
                EXPORT  irq0_int
irq0_int        FUNCTION
               SUB     LR, LR, #4            
                  STMFD   SP!, {R0-R3, R12, LR}
    MRS     R0, SPSR
               STMFD   SP!, {R0, R1}          
               MSR     CPSR_c, #0x1F        
               STMFD   SP!, {R0, LR}          
               LDR     R0, =irq0_nint        
               MOV     LR, PC
               BX      R0
               LDMFD   SP!, {R0, LR}          
               MSR     CPSR_c, #0x92          
               LDMFD   SP!, {R0, R1}          
               MSR     SPSR_cxsf, R0          
    LDMFD   SP!, {R0-R3, R12, PC}^  ;
               ENDFUNC
               END

void irq_pioa_nin(void){                
    unsigned int status = AT91C_BASE_SYS->PIOA_ISR;    
    
    if(status & ENCODER1_A)
        if ((pPIO->PIO_PDSR & ENCODER1_B) == 0) {
            ENCODER1_Position++;}
        else {
            ENCODER1_Position--;}                        
    if(status & ENCODER1_B)
        if ((pPIO->PIO_PDSR & ENCODER1_A) == 1) {
            ENCODER1_Position++;}
          else {
            ENCODER1_Position--;}                    
    AT91C_BASE_AIC->AIC_EOICR = status;
}

signed char enc_step[16] =
{
// Previous Current Increment
// AB AB
//
0, // 00 00
1, // 00 01 +1
-1, // 00 10 -1
0, // 00 11
-1, // 01 00 -1
0, // 01 01
0, // 01 10
1, // 01 11 +1
1, // 10 00 +1
0, // 10 01
0, // 10 10
-1, // 10 11 -1
0, // 11 00
-1, // 11 01 -1
1, // 11 10 +1
0 // 11 11
};

void irq_pioa_nin(void)
{
unsigned int status = AT91C_BASE_SYS->PIOA_ISR, a, b, pa, pb;


a = (AT91C_BASE_PIOA->PIO_PDSR & ENCODER1_A) ? 1 : 0;
b = (AT91C_BASE_PIOA->PIO_PDSR & ENCODER1_B) ? 1 : 0;
pa = (status & ENCODER_A) ? a ^ 1 : a;
pb = (status & ENCODER_B) ? b ^ 1 : b;


ENCODER1_Position += enc_step[(pa << 3) | (pb << 2) | (a << 1) | b];


AT91C_BASE_AIC->AIC_EOICR = status;
}