Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 

182 lines
3.5 KiB

#include "uart.h"
#include "types.h"
#include "interrupt.h"
#include "event.h"
#define UARTBASE 0xE000C000
#define RBR 0x00
#define THR 0x00
#define DLL 0x00
#define DLM 0x04
#define IER 0x04
#define IIR 0x08
#define FCR 0x08
#define LCR 0x0c
#define LSR 0x14
#define SCR 0x1c
#define ACR 0x20
#define FDR 0x28
#define TER 0x30
#define UREG(x) (((volatile unsigned char *)UARTBASE)[x])
#define U0THRE ((UREG(LSR) & (1<<5))) /* UART0 transmitter holding register is empty */
#define U0DR ((UREG(LSR) & (1<<0))) /* UART0 data ready */
#define UART_TXBUFSIZE 128
#define UART_RXBUFSIZE 128
char uart_txbuf[UART_TXBUFSIZE];
char uart_rxbuf[UART_RXBUFSIZE];
volatile int uart_txread;
volatile int uart_txwrite;
volatile int uart_rxread;
volatile int uart_rxwrite;
volatile bool tx_running;
void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void);
void init_uart(void)
{
UREG(FDR) = 0x10; /* DivAddVal = 0, MulVal = 1 */
UREG(LCR) = 0x80;
UREG(DLM) = 0x00;
UREG(DLL) = 0x08; /* 14745600 / (16*115200) */
UREG(LCR) = 0x13;
UREG(FCR) = 0x07;
uart_txread = 0;
uart_txwrite = 0;
uart_rxread = 0;
uart_rxwrite = 0;
tx_running = FALSE;
interrupt_register(UART0, uart_interrupt_handler);
UREG(IER) = 0x03; /* RBR and THRE interrupt enable */
}
void putch(char c) {
/* Wait for space in the buffer */
while (uart_txread == ((uart_txwrite+1) % UART_TXBUFSIZE));
if (uart_txread == uart_txwrite) {
if (U0THRE) {
tx_running = TRUE;
UREG(THR) = c;
return;
}
}
uart_txbuf[uart_txwrite] = c;
uart_txwrite = (uart_txwrite + 1) % UART_TXBUFSIZE;
if (!tx_running) {
if (uart_txread != uart_txwrite) {
tx_running = TRUE;
uart_txread = (uart_txread + 1) % UART_TXBUFSIZE;
UREG(THR) = c;
}
}
}
void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void)
{
bool active = FALSE;
int source;
int i;
/* uart_txread and uart_txwrite are volatile. We don't need
* to treat them as such in this handler, so let the compiler
* have an easier time.
*/
int local_txwrite;
int local_txread;
int local_rxwrite;
int local_rxread;
source = UREG(IIR);
switch (source & 0x0e) {
case 0x4: /* Receive Data Available */
case 0xc: /* Character Time-out Indicator */
local_rxwrite = uart_rxwrite;
local_rxread = uart_rxread;
while (U0DR) {
unsigned char c = UREG(RBR);
if (local_rxread !=
((local_rxwrite+1) % UART_RXBUFSIZE)) {
uart_rxbuf[local_rxwrite] = c;
local_rxwrite = (local_rxwrite + 1) %
UART_RXBUFSIZE;
}
}
uart_rxwrite = local_rxwrite;
event_set(EVENT_UART_INPUT);
break;
case 0x2: /* THRE interrupt */
local_txwrite = uart_txwrite;
local_txread = uart_txread;
for (i = 0; (i < 16) && (local_txwrite != local_txread); i++) {
UREG(THR) = uart_txbuf[local_txread];
local_txread = (local_txread + 1) % UART_TXBUFSIZE;
active = TRUE;
}
uart_txread = local_txread;
if (!active)
tx_running = FALSE;
break;
case 0x6: /* Receive Line Status */
default: /* Anything else */
break;
}
interrupt_clear();
}
void putstr(char *s) {
while (*s) putch(*s++);
}
void putint(unsigned int n) {
char s[11];
int i;
i = 10;
s[i] = '\0';
do {
s[--i] = n % 10 + '0';
} while ((n /= 10) > 0);
putstr(s+i);
}
void puthex(unsigned int n) {
char s[9];
int i;
i = 8;
s[i] = '\0';
do {
int x = n % 16;
if (x > 9)
x += 'A' - '0' - 10;
s[--i] = x + '0';
} while ((n /= 16) > 0);
putstr(s+i);
}
bool getch(char *c) {
if (uart_rxread == uart_rxwrite)
return FALSE;
*c = uart_rxbuf[uart_rxread];
uart_rxread = (uart_rxread + 1) % UART_RXBUFSIZE;
return TRUE;
}