printer: avr: Implement protocol with kernel driver

ioc/kernel-driver/lw35ioc.c

@@ -11,6 +11,7 @@
 #include <linux/uaccess.h>
 #include <linux/sched.h>
 #include <linux/poll.h>
+#include <linux/circ_buf.h>
 
 // #define ENABLE_DEBUG ENABLE_DEBUG
 
@@ -27,6 +28,11 @@ MODULE_PARM_DESC(debug_level, "Debug level");
 static struct serdev_device *static_serdev = NULL;
 static DECLARE_WAIT_QUEUE_HEAD(wq);
 
+void pbuf_work_handler(struct work_struct *work);
+DECLARE_WORK(pbuf_work, pbuf_work_handler);
+
+static DEFINE_SPINLOCK(printer_status_lock);
+
 static struct of_device_id lw35ioc_dt_ids[] = {
 { .compatible = "brother,lw35", },
 { /* sentinel */ }
@@ -37,7 +43,7 @@ MODULE_DEVICE_TABLE(of, lw35ioc_dt_ids);
 #define DEVICE_NAME "lw35"
 #define CLASS_NAME "lw35ioc"
 
-/* Only used for initialization, not for actualy keycode mapping */
+/* Only used for initialization, not for actual keycode mapping */
 static int keycode_map[] = {
 	KEY_A, KEY_B, KEY_C, KEY_D, KEY_E, KEY_F, KEY_G, KEY_H, KEY_I, KEY_J,
 	KEY_K, KEY_L, KEY_M, KEY_N, KEY_O, KEY_P, KEY_Q, KEY_R, KEY_S, KEY_T,
@@ -61,6 +67,7 @@ enum lw35ioc_packet_state {SYNC, TYPE, KEYBOARD, PRINTER};
 
 struct lw35ioc_packet {
 	enum lw35ioc_packet_state state;
+	int payload_offset;
 };
 
 struct lw35ioc_chardev {
@@ -70,7 +77,15 @@ struct lw35ioc_chardev {
 	int open_count;
 };
 
+struct lw35ioc_printer_status{
+	int buffer_free;
+	int status;
+	int reserved0;
+	int reserved1;
+};
+
 #define UART_BUF_SIZE 256
+#define PBUF_SIZE 256
 
 struct lw35ioc_priv {
 	struct serdev_device *serdev;
@@ -78,10 +93,17 @@ struct lw35ioc_priv {
 	struct gpio_desc *gpio_rst; /* GPIO to reset both AVRs */
 	struct lw35ioc_chardev chardev;
 
+	struct lw35ioc_printer_status printer_status_temp;
+	struct lw35ioc_printer_status printer_status;
+
 	char uart_buf_rx[UART_BUF_SIZE];
 	char *uart_buf_rx_rd;
 	char *uart_buf_rx_wr;
 
+	char pbuf[PBUF_SIZE];
+	unsigned long pbuf_tail;
+	unsigned long pbuf_head;
+
 	enum lw35ioc_state state;
 	struct lw35ioc_packet packet;
 };
@@ -95,6 +117,7 @@ static int lw35ioc_process_buf(struct serdev_device *serdev,
 	unsigned char c;
 	int ev;
 	int i;
+	unsigned long lock_flags;
 
 #ifdef ENABLE_DEBUG
 	dev_info(&serdev->dev, "IRQ.\n");
@@ -109,6 +132,7 @@ static int lw35ioc_process_buf(struct serdev_device *serdev,
 			if (c == 0xFF)
 				priv->packet.state = TYPE;
 			break;
+
 		case TYPE:
 			if (c == 0x55)
 				priv->packet.state = KEYBOARD;
@@ -116,7 +140,9 @@ static int lw35ioc_process_buf(struct serdev_device *serdev,
 				priv->packet.state = PRINTER;
 			else
 				priv->packet.state = SYNC;
+			priv->packet.payload_offset = 0;
 			break;
+
 		case KEYBOARD:
 			if (c > 127) {
 				c -= 128;
@@ -128,10 +154,47 @@ static int lw35ioc_process_buf(struct serdev_device *serdev,
 			input_sync(priv->input_dev);
 			priv->packet.state = SYNC;
 			break;
+
 		case PRINTER:
-			dev_info(&serdev->dev, "Printer not yet implemented.\n");
+			switch (priv->packet.payload_offset) {
+			case 0: /* Status */
+				priv->printer_status_temp.status = (int)c;
+				break;
+			case 1: /* Buffer status */
+				priv->printer_status_temp.buffer_free = (int)c;
+				break;
+			case 2: /* Reserved */
+				priv->printer_status_temp.reserved0 = (int)c;
+				break;
+			case 3: /* Reserved */
+				priv->printer_status_temp.reserved1 = (int)c;
+
+				dev_info(&serdev->dev,
+					 "New printer status. status=0x%02X buffer_free=0x%02X [0x%02X,0x%02X].\n",
+					 priv->printer_status_temp.status,
+					 priv->printer_status_temp.buffer_free,
+					 priv->printer_status_temp.reserved0,
+					 priv->printer_status_temp.reserved1);
+				if (priv->printer_status_temp.status == 0xFF) {
+					dev_err(&serdev->dev,
+						"HARDFAULT in printer firmware! Reset required.\n");
+				}
+				spin_lock_irqsave(&printer_status_lock, lock_flags);
+				memcpy(&priv->printer_status,
+				       &priv->printer_status_temp,
+				       sizeof(priv->printer_status));
+				spin_unlock_irqrestore(&printer_status_lock, lock_flags);
+
+				if (priv->printer_status_temp.buffer_free > 0)
+					schedule_work(&pbuf_work);
+
 				priv->packet.state = SYNC;
 				break;
+			default:
+				break;
+			}
+			priv->packet.payload_offset++;
+			break;
 		}
 	}
 
@@ -145,9 +208,14 @@ static int lw35ioc_receive_buf(struct serdev_device *serdev,
 	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
 	int i;
 
-	if (priv->state == ACTIVE)
+	switch (priv->state) {
+	case ACTIVE:
 		return lw35ioc_process_buf(serdev, data, count);
-
+		break;
+	case PASSIVE:
+		break;
+	default:
+		/* Pass data directly to chardev */
 		for (i = 0; i < count; ++i) {
 #ifdef ENABLE_DEBUG
 			dev_info(&serdev->dev, "  RX [PSV]: %02x\n", data[i]);
@@ -164,6 +232,7 @@ static int lw35ioc_receive_buf(struct serdev_device *serdev,
 		}
 
 		wake_up_interruptible(&wq);
+	}
 
 	return count; /* Pretend we read all data */
 }
@@ -205,32 +274,38 @@ static int lw35ioc_chardev_open(struct inode *inodep, struct file *filep) {
 	if (!priv->chardev.open_count) {
 		dev_info(&serdev->dev, "Opened chardev.\n");
 
-		if (priv->state != ACTIVE &&
-				priv->state != PASSIVE) {
-			lw35ioc_reset_ioc_hw(serdev);
-
 		switch (priv->state) {
 		case PROG_KBD:
+			lw35ioc_reset_ioc_hw(serdev);
 			serdev_device_write(serdev, "X", 1, 0);
 			dev_info(&serdev->dev, "Put ioc into programming mode: keyboard.\n");
 			break;
 		case PROG_PRI:
+			lw35ioc_reset_ioc_hw(serdev);
 			serdev_device_write(serdev, "Y", 1, 0);
 			dev_info(&serdev->dev, "Put ioc into programming mode: printer.\n");
 			break;
 		case BYPASS:
+			lw35ioc_reset_ioc_hw(serdev);
 			serdev_device_write(serdev, "XX", 2, 0);
 			dev_info(&serdev->dev, "Put ioc into bypass mode.\n");
 			break;
-			default:
-				/* For other modes, we just wait for the reset to be over */
+		case PASSIVE:
+			lw35ioc_reset_ioc_hw(serdev);
 			usleep_range(1000000, 1010000);
+			dev_info(&serdev->dev, "Put ioc into active mode.\n");
+			priv->state = ACTIVE;
+			break;
+		case ACTIVE:
+			/* Don't do anything when active */
+			break;
+		default:
 			break;
 		}
-		}
+
 	} else {
 		dev_err(&serdev->dev, "Rejected. Device may only be opened once.\n");
-		//return -EBUSY;
+		//return -EBUSY; // TODO
 	}
 
 	priv->chardev.open_count++;
@@ -251,7 +326,7 @@ static ssize_t lw35ioc_chardev_read(struct file *filep, char *buffer, size_t len
 		wait_event_interruptible(wq, priv->uart_buf_rx_rd != priv->uart_buf_rx_wr);
 		available = priv->uart_buf_rx_wr - priv->uart_buf_rx_rd;
 		if (available < 0)
-			available = 1;
+			available = 1; /* TODO: We should calculate the remaining block until wraparound here... */
 		if (!available)
 			return -EAGAIN;
 	}
@@ -279,12 +354,61 @@ char msg[256]; // TODO
 static ssize_t lw35ioc_chardev_write(struct file *filep, const char *buffer, size_t len, loff_t *offset) {
 	struct serdev_device *serdev = filep->private_data;
 	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+	unsigned long tail;
+	int i;
+	int ret;
 
+	switch (priv->state) {
+	case ACTIVE:
+		/* Put data into pbuf */
+		// TODO: spinlock
+		if (CIRC_SPACE(priv->pbuf_head, READ_ONCE(priv->pbuf_tail), PBUF_SIZE) < 1) {
+			dev_err(&serdev->dev, "chdev write: Waiting for space...\n");
+			if (ret = (wait_event_interruptible(wq, CIRC_SPACE(priv->pbuf_head, priv->pbuf_tail, PBUF_SIZE) >= 1))) { // TODO: Accessing pbuf_tail without any protection... this might be bad?
+				dev_err(&serdev->dev,
+					"chdev write: device closed while waiting. ret: %d\n",
+					ret);
+				// TODO: This is not proper handling of this I guess...
+				return len;
+			}
+			dev_err(&serdev->dev, "chdev write: done waiting...\n");
+		}
+
+		tail = READ_ONCE(priv->pbuf_tail);
+		for (i = 0; i < len; ++i) { // TODO: Block copy would be faster...
+			if (CIRC_SPACE(priv->pbuf_head, tail, PBUF_SIZE) >= 1) {
+				ret = copy_from_user(priv->pbuf + priv->pbuf_head,
+						     buffer + i,
+						     1);
+				if (ret) {
+					dev_err(&serdev->dev, "chdev write: copy_from_user failed (%d).\n",
+						ret);
+					i = len - 1;
+				}
+				smp_store_release(&(priv->pbuf_head),
+						  (priv->pbuf_head + 1) & (PBUF_SIZE - 1));
+			} else {
+				dev_warn(&serdev->dev, "chdev write: ring buffer full.\n");
+				break;
+			}
+		}
+		len = i;
+
+		schedule_work(&pbuf_work);
+		dev_info(&serdev->dev,
+			 "Copied %d bytes to FIFO (FIFO free: %d)",
+			 len, CIRC_SPACE(priv->pbuf_head, tail, PBUF_SIZE));
+		break;
+	case PASSIVE:
+		len = -EBUSY; // TODO: Can I do this?
+		break;
+	default: /* Pass-through */
 		if (len > 256) {
 			len = 256;
 		}
-	copy_from_user(msg, buffer, len);
+		copy_from_user(msg, buffer, len); // TODO: Return value
 		serdev_device_write_buf(serdev, msg, len);
+	}
 
 	return len;
 }
@@ -337,6 +461,75 @@ __poll_t poll(struct file *filep, struct poll_table_struct *ptab)
 	return 0;
 }
 
+#define PRINTER_READY_MASK (0b00011111)
+int printer_ready_for_data(struct lw35ioc_printer_status printer_status)
+{
+	return ((printer_status.status & PRINTER_READY_MASK) == PRINTER_READY_MASK);
+}
+
+void pbuf_work_handler(struct work_struct *work)
+{
+	// TODO: get private data using container_of(): https://linux-kernel-labs.github.io/master/labs/deferred_work.html
+	struct serdev_device *serdev = static_serdev; // Not cool, but whatever...
+	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+	unsigned long head;
+	unsigned long tail;
+	unsigned long lock_flags;
+	int bsize = 0;
+	int i;
+	int locked = 0;
+
+	dev_info(&serdev->dev,
+		 "pbuf_work_handler\n");
+
+	while (1) {
+		// TODO: We are the only consumer, so we should be fine without a spinlock, right?
+		head = smp_load_acquire(&priv->pbuf_head);
+		tail = priv->pbuf_tail;
+		bsize = CIRC_CNT(head, tail, PBUF_SIZE);
+		dev_info(&serdev->dev, "CIRC CNT=%d\n", bsize);
+		if (bsize == 0)
+			break;
+
+		spin_lock_irqsave(&printer_status_lock, lock_flags);
+		locked = 1;
+		if (!printer_ready_for_data(priv->printer_status)) {
+			dev_warn(&serdev->dev, "Cannot transmit data to device. Device is not ready.\n");
+			break;
+		}
+		if (bsize > priv->printer_status.buffer_free)
+			bsize = priv->printer_status.buffer_free;
+		if (bsize > CIRC_CNT_TO_END(head, tail, PBUF_SIZE))
+			bsize = CIRC_CNT_TO_END(head, tail, PBUF_SIZE);
+		if (bsize > priv->printer_status.buffer_free - 20)
+			bsize = priv->printer_status.buffer_free - 20; // TODO: Bug workaround
+		dev_info(&serdev->dev, "TX %d of %d bytes [%d bytes free on device].\n",
+			 bsize,
+			 CIRC_CNT(head, tail, PBUF_SIZE),
+			 priv->printer_status.buffer_free);
+		priv->printer_status.buffer_free-=bsize;
+		spin_unlock_irqrestore(&printer_status_lock, lock_flags);
+		locked = 0;
+		if (bsize == 0) {
+			dev_warn(&serdev->dev, "Cannot transmit data to device. Device buffer is full.\n");
+			break;
+		}
+
+		serdev_device_write_buf(serdev,
+					priv->pbuf + tail,
+					bsize);
+		for (i=0; i<bsize; ++i) {
+			dev_info(&serdev->dev,
+				 "UART TX: %c\n",
+				 priv->pbuf[tail + i]);
+		}
+		smp_store_release(&priv->pbuf_tail,
+				  (tail + bsize) & (PBUF_SIZE - 1));
+	}
+	if (locked)
+		spin_unlock_irqrestore(&printer_status_lock, lock_flags);
+	wake_up_interruptible(&wq); // There's some space available in the FIFO now, so wake up chardev write
+}
 
 
 static struct file_operations fops =
@@ -382,7 +575,7 @@ static int lw35ioc_cdev_init(struct serdev_device *serdev)
 		return PTR_ERR(priv->chardev.device);
 	}
 
-	dev_err(&serdev->dev, "Device class created correctly\n");
+	dev_info(&serdev->dev, "Device class created correctly\n");
 	return 0;
 }
 
@@ -414,11 +607,16 @@ static int lw35ioc_probe(struct serdev_device *serdev)
 	priv->uart_buf_rx_rd = priv->uart_buf_rx;
 	priv->uart_buf_rx_wr = priv->uart_buf_rx;
 
+	priv->pbuf_tail = 0;
+	priv->pbuf_head = 0;
+
+	priv->printer_status.buffer_free = 0;
+	priv->printer_status.status = 0;
+
 	priv->serdev = serdev;
 	static_serdev = serdev;
 	priv->packet.state = SYNC;
 	serdev_device_set_drvdata(serdev, priv);
-	serdev_device_set_client_ops(serdev, &lw35ioc_serdev_ops);
 
 	/* Register input device */
 	input_dev = devm_input_allocate_device(&serdev->dev);
@@ -440,12 +638,13 @@ static int lw35ioc_probe(struct serdev_device *serdev)
 		goto fail_input;
 	}
 
+	serdev_device_set_client_ops(serdev, &lw35ioc_serdev_ops);
+
 	ret = serdev_device_open(serdev);
 	if (ret) {
 		dev_err(&serdev->dev, "Unable to open device.\n");
 		goto fail_serdev;
 	}
-
 	speed = serdev_device_set_baudrate(serdev, speed);
 	dev_info(&serdev->dev, "Using baudrate: %u\n", speed);
 
@@ -486,17 +685,26 @@ static void lw35ioc_remove(struct serdev_device *serdev)
 {
 	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
 
+	dev_info(&serdev->dev, "remove\n");
+	dev_info(&serdev->dev, "  cdev class: %p\n", priv->chardev.class);
+
 	device_destroy(priv->chardev.class, MKDEV(priv->chardev.major_number, 0));
+	dev_info(&serdev->dev, "unreg\n");
 	class_unregister(priv->chardev.class);
-	class_destroy(priv->chardev.class);
+	dev_info(&serdev->dev, "destroy: SKIP\n");
+	//class_destroy(priv->chardev.class);
+	dev_info(&serdev->dev, "unreg-dev\n");
 	unregister_chrdev(priv->chardev.major_number, DEVICE_NAME);
 
+	dev_info(&serdev->dev, "  gpio rst: %p\n", priv->gpio_rst);
+	dev_info(&serdev->dev, "  serdev: %p\n", priv->serdev);
+	dev_info(&serdev->dev, "  input dev: %p\n", priv->input_dev);
 	lw35ioc_set_hw_reset(serdev, 1);
 	gpiod_put(priv->gpio_rst);
 	serdev_device_close(priv->serdev);
 	input_unregister_device(priv->input_dev);
 
-	dev_info(&serdev->dev, "remove\n");
+	dev_info(&serdev->dev, "done.\n");
 	return;
 }
 

ioc/lw35-tool/CMakeLists.txt

@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(lw35-tool LANGUAGES C)
+
+add_executable(lw35-tool main.c)

keyboard/avr/main.c

@@ -14,7 +14,7 @@
 
 #define BAUDRATE 19200
 
-#define PRINTER_PACKET_SIZE 4
+#define PRINTER_PACKET_SIZE 6
 
 /* Keycode from include/linux/input.h */
 #define KEY_RESERVED		0
@@ -214,7 +214,8 @@ void uart_putc(char c)
 ISR(USART_RXC_vect)
 {
 	if (UCSRA & (1 << RXC)) {
-		ring_buffer[ring_buffer_wr++] = UDR;// WARNING: No overflow protection
+		ring_buffer[ring_buffer_wr] = UDR;
+		ring_buffer_wr++; // WARNING: No overflow protection
 		// Will auto wrap at 256; print_buffer_wr = print_buffer_wr % PRINT_BUFFER_SIZE;
 	}
 }
@@ -229,11 +230,20 @@ void uart_forward_packets()
 	else
 		size = ring_buffer_rd - ring_buffer_wr;
 
-	if (size > PRINTER_PACKET_SIZE) {
-		uart_putc(0xFF);
-		uart_putc(0xAA);
+	if (size >= PRINTER_PACKET_SIZE) {
+		/* check sync */
+		if (ring_buffer[ring_buffer_rd] != ((char)0xFF)) {
+			ring_buffer_rd++; /* Drop wrong sync header */
+			return;
+		}
+		if (ring_buffer[ring_buffer_rd + 1] != ((char)0xAA)) {
+			ring_buffer_rd += 1; /* Drop wrong app header */
+			return;
+		}
+
 		for (i = 0; i < PRINTER_PACKET_SIZE; ++i) {
-			uart_putc(ring_buffer[ring_buffer_rd++]); // Will auto wrap at 256
+			uart_putc(ring_buffer[ring_buffer_rd]); // Will auto wrap at 256
+			ring_buffer_rd++;
 		}
 	}
 }

printer/avr/main.c

@@ -5,6 +5,7 @@
 #include <avr/pgmspace.h>
 
 #define BAUDRATE 19200
+//#define DEBUG_BUILD DEBUG_BUILD
 
 /* System functions */
 /* Timers */
@@ -53,8 +54,7 @@ void uart_putc(char c)
 	UDR = c;
 }
 
-#define ENABLE_SERIAL_DEBUG
-#ifdef ENABLE_SERIAL_DEBUG
+#ifdef DEBUG_BUILD
 void debug_putc(char c) {
 	uart_putc(c);
 };
@@ -90,10 +90,20 @@ unsigned int debug_printf(char *format,...) {return 0;};
 
 void hardfault() {
 	cli();
-	debug_write("HARDFAULT!\r\n");
 	PORTB = 0;
 	PORTC = 0;
 	PORTD = 0;
+	uart_putc(0xFF);
+	uart_putc(0xFF);
+	uart_putc(0xFF);
+	uart_putc(0xFF);
+	uart_putc(0xFF);
+	uart_putc(0xFF);
+	uart_putc(0xFF);
+	uart_putc(0x00);
+	uart_putc(0x00);
+	uart_putc(0x00);
+	debug_write("HARDFAULT!\r\n");
 	while(1);
 }
 
@@ -560,16 +570,99 @@ void arm_correction()
 
 #define DCMOTOR_EN MOTLIM(PORT) |= PIN_DCMOTOR
 #define DCMOTOR_STOP MOTLIM(PORT) &= ~PIN_DCMOTOR
-#define DCMOTOR_ISACTIVE (MOTLIM(PORT) & PIN_DCMOTOR)
+#define DCMOTOR_ISACTIVE (!!(MOTLIM(PORT) & PIN_DCMOTOR))
 #define LIMITSWITCH (!(MOTLIM(PIN) & PIN_LIMITSWITCH))
 
+/* Communication Functions */
+#define RINGBUF_CNT(wr,rd) ((wr) - (rd))
+#define RINGBUF_FREE(wr,rd,sz) ((RINGBUF_CNT(rd,wr+1))) // TODO: 255 is actually not correct...
+#define PRINT_BUFFER_SIZE 256 // Important: must be 256 atm, when changed, you need to change code handling print_buffer_*.
+volatile char print_buffer[PRINT_BUFFER_SIZE];
+volatile uint8_t print_buffer_wr = 0;
+volatile uint8_t print_buffer_rd = 0;
+
+#define UART_BYTE_READY (UCSRA & (1 << RXC))
+
+ISR(USART_RXC_vect)
+{
+	if (UCSRA & (1 << RXC)) {
+		print_buffer[print_buffer_wr++] = UDR;// WARNING: No overflow protection
+		// Will auto wrap at 256; print_buffer_wr = print_buffer_wr % PRINT_BUFFER_SIZE;
+	}
+}
+
+#define TX_BUFFER_SIZE 256 // Important: must be 256 atm, when changed, you need to change code handling print_buffer_*.
+char tx_buffer[TX_BUFFER_SIZE];
+uint8_t tx_buffer_wr = 0;
+uint8_t tx_buffer_rd = 0;
+uint8_t tx_buffer_async = 0;
+
+uint8_t global_status;
+#define STATUS_PAPERFEED (1 << 0)
+#define STATUS_CARRIAGE (1 << 1)
+#define STATUS_DAISY (1 << 2)
+#define STATUS_PRINTHEAD (1 << 3)
+#define STATUS_READY (1 << 4)
+#define STATUSBIT_ACTIVE 5
+
+void tx_buffer_putc(char c)
+{
+	if (tx_buffer_async) {
+		if (tx_buffer_rd == tx_buffer_wr + 1) // implicit % 256
+			hardfault();
+		tx_buffer[tx_buffer_wr++] = c; // implicit % 256
+	} else {
+		uart_putc(c);
+	}
+}
+
+void tx_status()
+{
+	unsigned char local_status = 0;
+	local_status |= (DCMOTOR_ISACTIVE << STATUSBIT_ACTIVE);
+
+	tx_buffer_putc(0xFF); /* Header[0] */
+	tx_buffer_putc(0xAA); /* Header[1] */
+	tx_buffer_putc((char) (global_status | local_status));
+	tx_buffer_putc(RINGBUF_FREE(print_buffer_wr, print_buffer_rd, PRINT_BUFFER_SIZE));
+	tx_buffer_putc(0); /* RESERVED */
+	tx_buffer_putc(0); /* RESERVED */
+}
+
+void update_status(uint8_t status)
+{
+	global_status = status;
+
+	tx_status();
+}
+
+void set_status(uint8_t bit)
+{
+	update_status(global_status | bit);
+}
+
+void clear_status(uint8_t bit)
+{
+	update_status(global_status & ~bit);
+}
+
+void tx_buffer_process()
+{
+	if (tx_buffer_rd != tx_buffer_wr) {
+		if (UCSRA & (1 << UDRE)) {
+			UDR = tx_buffer[tx_buffer_rd];
+			tx_buffer_rd++; // implicit % 256
+		}
+	}
+}
 
 /* Initialization Functions */
-
 void move_carriage_to_far_left(uint8_t reset)
 {
 	uint16_t cnt = 0;
 
+	clear_status(STATUS_CARRIAGE);
+
 	/* Init stepper controller */
 	if (reset) {
 		stepper_status_CARRIAGE.step = 0;
@@ -605,6 +698,7 @@ void move_carriage_to_far_left(uint8_t reset)
 	stepper_status_CARRIAGE.target_pos = 0;
 
 	debug_printf("[car] Carriage left after %u steps.\r\n", cnt);
+	set_status(STATUS_CARRIAGE);
 }
 
 void align_daisy_wheel()
@@ -612,6 +706,8 @@ void align_daisy_wheel()
 	int i;
 
 	debug_write("[whl] Aligning wheel...\r\n");
+	clear_status(STATUS_DAISY);
+
 	stepper_status_WHEEL.dir = 0;
 	stepper_status_WHEEL.lpstate = 0;
 	stepper_status_WHEEL.pos = 0;
@@ -651,15 +747,18 @@ void align_daisy_wheel()
 	stepper_status_WHEEL.pos = 0;
 
 	debug_write("[whl] Alignment completed.\r\n");
+	set_status(STATUS_DAISY);
 }
 
 void reset_printhead()
 {
 	debug_write("[hmr] Resetting printhead...\r\n");
+	clear_status(STATUS_PRINTHEAD);
 	DCMOTOR_EN;
 	_delay_ms(200);
 	DCMOTOR_STOP;
 	debug_write("[hmr] Printhead reset completed.\r\n");
+	set_status(STATUS_PRINTHEAD);
 }
 
 void initialize_paperfeed()
@@ -667,6 +766,7 @@ void initialize_paperfeed()
 	int i;
 
 	debug_write("[lfd] Initializing paperfeed...\r\n");
+	clear_status(STATUS_PAPERFEED);
 
 	for (i = 0; i < 10;) {
 		if (block_for(TIM_LINEFEED, STEPPER_CFG_LINEFEED.delay)) {
@@ -684,26 +784,10 @@ void initialize_paperfeed()
 	stepper_status_LINEFEED.ldir = 0;
 
 	debug_write("[lfd] Initialization completed.\r\n");
+	set_status(STATUS_PAPERFEED);
 }
 
 /* Printer Functions */
-
-#define PRINT_BUFFER_SIZE 256 // Important: must be 256 atm, when changed, you need to change code handling print_buffer_*.
-volatile char print_buffer[PRINT_BUFFER_SIZE];
-volatile uint8_t print_buffer_wr = 0;
-volatile uint8_t print_buffer_rd = 0;
-
-#define UART_BYTE_READY (UCSRA & (1 << RXC))
-
-ISR(USART_RXC_vect)
-{
-	if (UCSRA & (1 << RXC)) {
-		print_buffer[print_buffer_wr++] = UDR;// WARNING: No overflow protection
-		// Will auto wrap at 256; print_buffer_wr = print_buffer_wr % PRINT_BUFFER_SIZE;
-	}
-}
-
-
 #define PRINT_CHARACTER_WIDTH 10
 #define PRINT_LINE_HEIGHT 34
 #define PRINTER_STOP_RIGHT 1100
@@ -841,6 +925,7 @@ void printer_process()
 				} else {
 					current_char = print_buffer[print_buffer_rd];
 					print_buffer_rd++;
+					tx_status(); // Tell SoC when bytes are consumed from the buffer. TODO: Too often?
 				}
 			} else {
 				current_char = '\0';
@@ -850,31 +935,30 @@ void printer_process()
 			current_is_linebreak += (current_char == '\n');
 			if (current_is_linebreak) {
 				if (backward) {
-					uart_putc('=');
+					debug_putc('=');
 					print_buffer_rd = print_buffer_recovery;
+					tx_status();
 				}
 				backward = 0; // Re-evaluate backward. Default is forward.
 
 				stepper_status_LINEFEED.target_pos += PRINT_LINE_HEIGHT;
-				uart_putc('\n');
+				debug_putc('\n');
 				current_is_linebreak--;
-				//_delay_ms(1000);
 
 				/* Decide whether we move the carriage back to the left, or
-				 * whether print the new line in reverse. */
+				 * whether to print the new line in reverse. */
 
 				end_of_next_line = print_buffer_rd;
 				temp = simulate_print_run(&end_of_next_line, &print_buffer_recovery);
 
 				if (temp == -1) {
-					//uart_putc('\r');
-					uart_putc('[');
+					debug_putc('[');
 					backward = 1;
 					print_buffer_backward = end_of_next_line;
 
 				} else if (temp == 0) {
 					// do nothing
-					uart_putc('#');
+					debug_putc('#');
 
 				} else {
 					move_carriage_to_left_margin();
@@ -891,7 +975,7 @@ void printer_process()
 				case PRINTER_NO_CHAR:
 					current_is_linebreak += move_one_character(backward);
 					if (!current_is_linebreak)
-						uart_putc(' ');
+						debug_putc(' ');
 					break;
 
 				default: /* It's a printable character */
@@ -912,7 +996,7 @@ void printer_process()
 		if (POSITION_REACHED(WHEEL) && POSITION_REACHED(CARRIAGE) && POSITION_REACHED(LINEFEED)) {
 			arm_hammer();
 			_delay_ms(50); // TODO: replace with timer
-			uart_putc(debug_character);
+			debug_putc(debug_character);
 			current_is_linebreak += move_one_character(backward);
 			state = IDLE;
 		}
@@ -943,7 +1027,16 @@ void printer_process()
 	stepper_perform_movement(&stepper_status_WHEEL, &STEPPER_CFG_WHEEL);
 }
 
+void sync_uart()
+{
+	int i;
+	for (i = 0; i < 10; ++i) {
+		tx_buffer_putc(0xFF);
+	}
+}
+
 /* Test Functions */
+#ifdef DEBUG_BUILD
 void systick_test()
 {
 	debug_write("[tmr] System timer test\r\n");
@@ -1135,11 +1228,17 @@ static void system_test_auto()
 		}
 	}
 }
+#endif
 
 void mainloop()
 {
+	tx_buffer_async = 1;
+	uart_irq_enable(); /* rx buffer process */
+	set_status(STATUS_READY);
+
 	while (1) {
 		printer_process();
+		tx_buffer_process();
 	}
 }
 
@@ -1181,24 +1280,26 @@ int main()
 	TIMSK = (1 << OCIE1A);
 
 	/* Init system */
+	sync_uart();
+	update_status(0);
 	debug_write("\n\n\r[sys] STARTING IO CONTROLLER...\r\n");
 	debug_write("[sys] Enabling interrupts.\r\n");
 	sei();
 
 	/* Align printer */
-	//initialize_paperfeed();
-	//move_carriage_to_far_left(1);
-	//align_daisy_wheel();
-	//reset_printhead();
+	initialize_paperfeed();
+	move_carriage_to_far_left(1);
+	align_daisy_wheel();
+	reset_printhead();
 
 	debug_write("[sys] Startup completed.\r\n");
 
 	/* Run system */
+#ifdef DEBUG_BUILD
 	system_test_auto();
-	// mainloop();
-	//	printer_test();
-	//	system_test_auto();
-	//	systick_test();
+#else
+	mainloop();
+#endif
 
 	debug_write("[sys] REACHED END OF MAIN. HALTING.\r\n");
 	while (1);