1 files changed, 26 insertions, 31 deletions

diff --git a/src/arm/PL1/PL1_common/uart.c b/src/arm/PL1/PL1_common/uart.c
index 4dd1c2b..94aae46 100644
--- a/src/arm/PL1/PL1_common/uart.c
+++ b/src/arm/PL1/PL1_common/uart.c
@@ -3,53 +3,48 @@
 #include "uart.h"
 #include "global.h"
 
-// Loop <delay> times in a way that the compiler won't optimize away
-static inline void delay(int32_t count)
+void uart_init(uint32_t baud, uint32_t config)
 {
-  asm volatile("__delay_%=: subs %[count], %[count], #1; bne __delay_%=\n"
-	       : "=r"(count): [count]"0"(count) : "cc");
-}
-
-void uart_init()
-{
-  // Disable PL011_UART.
+  // PL011 UART must be disabled before configuring
   wr32(PL011_UART_CR, 0);
-  
-  // Setup the GPIO pin 14 && 15.
 
-  // Disable pull up/down for all GPIO pins & delay for 150 cycles.
+  // GPIO pins used for UART should have pull up/down disabled
+  // Procedure as described in BCM2835 ARM Peripherals
   wr32(GPPUD, 0);
-  delay(150);
 
-  // Disable pull up/down for pin 14,15 & delay for 150 cycles.
+  for (int i = 0; i < 150; i++) // delay for at least 150 cycles
+    asm volatile("nop");
+
   wr32(GPPUDCLK0, (1 << 14) | (1 << 15));
-  delay(150);
 
-  // Write 0 to GPPUDCLK0 to make it take effect.
-  wr32(GPPUDCLK0, 0);
+  for (int i = 0; i < 150; i++)
+    asm volatile("nop");
 
-  // Set integer & fractional part of baud rate.
-  // Divider = UART_CLOCK/(16 * Baud)
-  // Fraction part register = (Fractional part * 64) + 0.5
-  // UART_CLOCK = 3000000; Baud = 115200.
+  wr32(GPPUDCLK0, 0);
+  
+  wr32(GPPUD, 0);  
 
-  // Divider = 3000000 / (16 * 115200) = 1.627 = ~1.
-  wr32(PL011_UART_IBRD, 1);
-  // Fractional part register = (.627 * 64) + 0.5 = 40.6 = ~40.
-  wr32(PL011_UART_FBRD, 40);
+  // Setting clock rate
+  // As described in UART (PL011) Technical Reference Manual
+  uint32_t int_part = DEFAULT_UART_CLOCK_RATE / (16 * baud);
+  uint32_t rest = DEFAULT_UART_CLOCK_RATE % (16 * baud);
+  uint32_t fract_part = (rest * 64 * 2 + 1) / (2 * 16 * baud);
+  
+  wr32(PL011_UART_IBRD, int_part);
+  wr32(PL011_UART_FBRD, fract_part);
 
-  // Set 8 bit data transmission (1 stop bit, no parity)
-  // and disable FIFO to be able to receive interrupt every received
+  // Set data transmission specified by caller
+  // Don't enable FIFO to be able to receive interrupt every received
   // char, not every 2 chars
-  wr32(PL011_UART_LCRH, (1 << 5) | (1 << 6));
+  wr32(PL011_UART_LCRH, config);
 
-  // set interrupt to come when transmit FIFO becomes ≤ 1/8 full
+  // Set interrupt to come when transmit FIFO becomes ≤ 1/8 full
   // or receive FIFO becomes ≥ 1/8 full
   // (not really matters, since we disabled FIFOs)
   wr32(PL011_UART_IFLS, 0);
 
-  // Enable PL011_UART, receive & transfer part of UART.2
-  wr32(PL011_UART_CR, (1 << 0) | (1 << 8) | (1 << 9));
+  // Enable UART receiving and transmitting, as well as UART itself
+  wr32(PL011_UART_CR, (1 << 9) | (1 << 8) | 1);
 
   // At first, it's probably safer to disable interrupts :)
   uart_irq_disable();