#define PS_BASE 0xFFFF4000

#define PS_CR (*(volatile unsigned *)0xFFFF4000)
#define PS_CR_OFFSET 0x0

#define PS_PCER (*(volatile unsigned *)0xFFFF4004)
#define PS_PCER_OFFSET 0x4

#define PS_PCDR (*(volatile unsigned *)0xFFFF4008)
#define PS_PCDR_OFFSET 0x8

#define PS_PCSR (*(volatile unsigned *)0xFFFF400C)
#define PS_PCSR_OFFSET 0xC

#define PIO_BASE 0xFFFF0000

#define PIO_PER (*(volatile unsigned *)0xFFFF0000)
#define PIO_PER_OFFSET 0x0

#define PIO_PDR (*(volatile unsigned *)0xFFFF0004)
#define PIO_PDR_OFFSET 0x4

#define PIO_PSR (*(volatile unsigned *)0xFFFF0008)
#define PIO_PSR_OFFSET 0x8

#define PIO_OER (*(volatile unsigned *)0xFFFF0010)
#define PIO_OER_OFFSET 0x10

#define PIO_ODR (*(volatile unsigned *)0xFFFF0014)
#define PIO_ODR_OFFSET 0x14

#define PIO_OSR (*(volatile unsigned *)0xFFFF0018)
#define PIO_OSR_OFFSET 0x18

#define PIO_IFER (*(volatile unsigned *)0xFFFF0020)
#define PIO_IFER_OFFSET 0x20

#define PIO_IFDR (*(volatile unsigned *)0xFFFF0024)
#define PIO_IFDR_OFFSET 0x24

#define PIO_IFSR (*(volatile unsigned *)0xFFFF0028)
#define PIO_IFSR_OFFSET 0x28

#define PIO_SODR (*(volatile unsigned *)0xFFFF0030)
#define PIO_SODR_OFFSET 0x30

#define PIO_CODR (*(volatile unsigned *)0xFFFF0034)
#define PIO_CODR_OFFSET 0x34

#define PIO_ODSR (*(volatile unsigned *)0xFFFF0038)
#define PIO_ODSR_OFFSET 0x38

#define PIO_PDSR (*(volatile unsigned *)0xFFFF003C)
#define PIO_PDSR_OFFSET 0x3C

#define PIO_IER (*(volatile unsigned *)0xFFFF0040)
#define PIO_IER_OFFSET 0x40

#define PIO_IDR (*(volatile unsigned *)0xFFFF0044)
#define PIO_IDR_OFFSET 0x44

#define PIO_IMR (*(volatile unsigned *)0xFFFF0048)
#define PIO_IMR_OFFSET 0x48

#define PIO_ISR (*(volatile unsigned *)0xFFFF004C)
#define PIO_ISR_OFFSET 0x4C

#define USART1_BASE 0xFFFCC000

#define US1_CR (*(volatile unsigned *)0xFFFCC000)
#define US1_CR_OFFSET 0x0

#define US1_MR (*(volatile unsigned *)0xFFFCC004)
#define US1_MR_OFFSET 0x4

#define US1_IER (*(volatile unsigned *)0xFFFCC008)
#define US1_IER_OFFSET 0x8

#define US1_IDR (*(volatile unsigned *)0xFFFCC00C)
#define US1_IDR_OFFSET 0xC

#define US1_IMR (*(volatile unsigned *)0xFFFCC010)
#define US1_IMR_OFFSET 0x10

#define US1_CSR (*(volatile unsigned *)0xFFFCC014)
#define US1_CSR_OFFSET 0x14

#define US1_RHR (*(volatile unsigned *)0xFFFCC018)
#define US1_RHR_OFFSET 0x18

#define US1_THR (*(volatile unsigned *)0xFFFCC01C)
#define US1_THR_OFFSET 0x1C

#define US1_BRGR (*(volatile unsigned *)0xFFFCC020)
#define US1_BRGR_OFFSET 0x20

#define US1_RTOR (*(volatile unsigned *)0xFFFCC024)
#define US1_RTOR_OFFSET 0x24

#define US1_TTGR (*(volatile unsigned *)0xFFFCC028)
#define US1_TTGR_OFFSET 0x28

#define US1_RPR (*(volatile unsigned *)0xFFFCC030)
#define US1_RPR_OFFSET 0x30

#define US1_RCR (*(volatile unsigned *)0xFFFCC034)
#define US1_RCR_OFFSET 0x34

#define US1_TPR (*(volatile unsigned *)0xFFFCC038)
#define US1_TPR_OFFSET 0x38

#define US1_TCR (*(volatile unsigned *)0xFFFCC03C)
#define US1_TCR_OFFSET 0x3C

#define USART0_BASE 0xFFFD0000

#define US0_CR (*(volatile unsigned *)0xFFFD0000)
#define US0_CR_OFFSET 0x0

#define US0_MR (*(volatile unsigned *)0xFFFD0004)
#define US0_MR_OFFSET 0x4

#define US0_IER (*(volatile unsigned *)0xFFFD0008)
#define US0_IER_OFFSET 0x8

#define US0_IDR (*(volatile unsigned *)0xFFFD000C)
#define US0_IDR_OFFSET 0xC

#define US0_IMR (*(volatile unsigned *)0xFFFD0010)
#define US0_IMR_OFFSET 0x10

#define US0_CSR (*(volatile unsigned *)0xFFFD0014)
#define US0_CSR_OFFSET 0x14

#define US0_RHR (*(volatile unsigned *)0xFFFD0018)
#define US0_RHR_OFFSET 0x18

#define US0_THR (*(volatile unsigned *)0xFFFD001C)
#define US0_THR_OFFSET 0x1C

#define US0_BRGR (*(volatile unsigned *)0xFFFD0020)
#define US0_BRGR_OFFSET 0x20

#define US0_RTOR (*(volatile unsigned *)0xFFFD0024)
#define US0_RTOR_OFFSET 0x24

#define US0_TTGR (*(volatile unsigned *)0xFFFD0028)
#define US0_TTGR_OFFSET 0x28

#define US0_RPR (*(volatile unsigned *)0xFFFD0030)
#define US0_RPR_OFFSET 0x30

#define US0_RCR (*(volatile unsigned *)0xFFFD0034)
#define US0_RCR_OFFSET 0x34

#define US0_TPR (*(volatile unsigned *)0xFFFD0038)
#define US0_TPR_OFFSET 0x38

#define US0_TCR (*(volatile unsigned *)0xFFFD003C)
#define US0_TCR_OFFSET 0x3C

// US_CR
#define RXEN  4
#define TXEN  6

// US_MR
#define CLK0    18
#define MODE9   17
#define CHMODE1 15
#define CHMODE0 14
#define NBSTOP1 13
#define NBSTOP0 12
#define PAR2    11
#define PAR1    10
#define PAR0    9
#define SYNC    8
#define CHRL1   7
#define CHRL0   6
#define USCLKS1 5
#define USCLKS0 4

// US_CSR
#define RXRDY   0
#define TXRDY   1
#define TXEMPTY 9

// PIO
#define P14_TXD0  14
#define P15_RXD0  15
#define P21_TXD1  21
#define P22_RXD1  22

void uart0Init(void)
{
  // Define RXD and TXD as peripheral
  PIO_PDR |=   (1<<P14_TXD0) | (1<<P15_RXD0);
  PIO_PER &= ~((1<<P14_TXD0) | (1<<P15_RXD0));

  // First, enable the clock
  // US0
  PS_PCER |= (1<<2);

  // Reset receiver and transmitter
  //       COM0->US_CR = AT91C_US_RSTRX | AT91C_US_RSTTX;

  // Reset Status Bits (PARE, FRAME, OVRE and RXBRK)
  //       COM0->US_CR = AT91C_US_RSTSTA;

  // Baudrate
  US0_BRGR = (66000000UL / 4800UL) / 16; 

  // async, 8 Bit, N, 1, normal mode, 
  US0_MR = 0 | (1<<CHRL1) | (1<<CHRL0) | (1<<PAR2);

  // Stop Peripheral Data Transfer
  US0_RCR = 0;
  US0_TCR = 0;

  // RX, TX enable
  US0_CR = 0 | (1<<RXEN) | (1<<TXEN);
}

static void uart0Putch(unsigned char data)
{
  while(!(US0_CSR & ((1<<TXRDY)|(1<<TXEMPTY))));
  US0_THR = (unsigned long) data;
}

static void uart0Puts(unsigned char *string)
{
  while(*string)
    uart0Putch(*string++);
}

static unsigned char uart0Getch(void)
{
  unsigned char data = '+';
  while(!(US0_CSR & (1<<RXRDY)));
  data = (unsigned char) (US0_RHR & 0x000000FF);
  return data;
}

int main(void)
{
  PIO_PDR |=   (1<<P21_TXD1) | (1<<P22_RXD1);
  PIO_PER &= ~((1<<P21_TXD1) | (1<<P22_RXD1));

  PIO_PER |= (1<<16);   // ENABLE PIO CONTROL PIN16
  PIO_OER |= (1<<16);   // ENABLE PIO OUTPUT
  PIO_SODR |= (1<<16);  // SET PIO OUTPUT

  uart0Init();

  uart0Putch('\f');
  uart0Puts("\r\nHello World!\r\n");

  while(1) {
    unsigned char ch = '#';
    ch = uart0Getch();

    if (ch >= 'a' && ch <= 'z')
      ch = ch - 'a' + 'A';
    else if (ch >= 'A' && ch <= 'Z')
      ch = ch - 'A' + 'a';
      
    uart0Putch(ch);
  }

  return 0;
}