components/SerialServer/src/serial.c - 3p/sel4proj/global-components - Git at Google

 /*
  * Copyright 2017, Data61, CSIRO (ABN 41 687 119 230)
  *
  * SPDX-License-Identifier: BSD-2-Clause
  */

 #include <autoconf.h>
 #include <camkes.h>
 #include <stdio.h>
 #include <stdint.h>
 #include <string.h>

 #include <sel4/sel4.h>
 #include <utils/attribute.h>
 #include <utils/ansi.h>
 #include <camkes.h>
 #include <camkes/io.h>
 #include <camkes/irq.h>
 #include <camkes/virtqueue.h>
 #include <platsupport/chardev.h>
 #include <platsupport/irq.h>
 #include "serial.h"
 #include "plat.h"
 #include "server_virtqueue.h"

 #define ESCAPE_CHAR '@'
 #define MAX_CLIENTS 12
 #define CLIENT_OUTPUT_BUFFER_SIZE 4096

 /* TODO: have the MultiSharedData template generate a header with these */
 void getchar_emit(unsigned int id) WEAK;
 seL4_Word getchar_enumerate_badge(unsigned int id) WEAK;
 unsigned int getchar_num_badges() WEAK;
 void *getchar_buf(unsigned int id) WEAK;
 void *processed_batch_buf(unsigned int id) WEAK;
 void *raw_batch_buf(unsigned int id) WEAK;
 int getchar_largest_badge(void) WEAK;

 typedef struct getchar_buffer {
     uint32_t head;
     uint32_t tail;
     char buf[4096 - 8];
 } client_buffer_t;

 compile_time_assert(getchar_buf_sized, sizeof(client_buffer_t) == sizeof(Buf));

 typedef struct getchar_client {
     unsigned int client_id;
     volatile client_buffer_t *buf;
     uint32_t last_head;
 } getchar_client_t;

 static ps_io_ops_t io_ops;

 static int last_out = -1;

 static uint8_t output_buffers[MAX_CLIENTS * 2][CLIENT_OUTPUT_BUFFER_SIZE];
 static int output_buffers_used[MAX_CLIENTS * 2] = { 0 };
 static uint16_t output_buffer_bitmask = 0;

 static int done_output = 0;

 static int has_data = 0;

 static int num_getchar_clients = 0;
 static getchar_client_t *getchar_clients = NULL;

 /* We predefine output colours for clients */
 const char *all_output_colours[2][MAX_CLIENTS] = {
     {
         /* Processed streams */
         ANSI_COLOR(RED),
         ANSI_COLOR(GREEN),
         ANSI_COLOR(BLUE),
         ANSI_COLOR(MAGENTA),
         ANSI_COLOR(YELLOW),
         ANSI_COLOR(CYAN),
         ANSI_COLOR(RED, BOLD)
         ANSI_COLOR(GREEN, BOLD),
         ANSI_COLOR(BLUE, BOLD),
         ANSI_COLOR(MAGENTA, BOLD),
         ANSI_COLOR(YELLOW, BOLD),
         ANSI_COLOR(CYAN, BOLD),
     },
     {
         /* Raw streams */
         ANSI_COLOR2(RED, WHITE),
         ANSI_COLOR2(GREEN, WHITE),
         ANSI_COLOR2(BLUE, WHITE),
         ANSI_COLOR2(MAGENTA, WHITE),
         ANSI_COLOR2(YELLOW, WHITE),
         ANSI_COLOR2(CYAN, WHITE),
         ANSI_COLOR2(RED, WHITE, BOLD)
         ANSI_COLOR2(GREEN, WHITE, BOLD),
         ANSI_COLOR2(BLUE, WHITE, BOLD),
         ANSI_COLOR2(MAGENTA, WHITE, BOLD),
         ANSI_COLOR2(YELLOW, WHITE, BOLD),
         ANSI_COLOR2(CYAN, WHITE, BOLD),
     },
 };

 #define COLOUR_INDEX_TO_STYLE(x) ((x) / (MAX_CLIENTS - 1))
 #define COLOUR_INDEX_TO_SLOT(x)  ((x) % MAX_CLIENTS)

 static void flush_buffer(int b)
 {
     const char *col = all_output_colours[COLOUR_INDEX_TO_STYLE(b)][COLOUR_INDEX_TO_SLOT(b)];
     int i;
     if (output_buffers_used[b] == 0) {
         return;
     }
     if (b != last_out) {
         printf("%s%s", COLOR_RESET, col);
         last_out = b;
     }
     for (i = 0; i < output_buffers_used[b]; i++) {
         printf("%c", output_buffers[b][i]);
     }
     done_output = 1;
     output_buffers_used[b] = 0;
     output_buffer_bitmask &= ~BIT(b);
     fflush(stdout);
 }

 static int debug = 0;

 /* Try to flush up to the end of the line. */
 static bool flush_buffer_line(int b)
 {
     if (output_buffers_used[b] == 0) {
         return 0;
     }
     uint8_t *nlptr = memchr(output_buffers[b], '\r', output_buffers_used[b]);
     if (nlptr == NULL) {
         nlptr = memchr(output_buffers[b], '\n', output_buffers_used[b]);
     }
     if (nlptr == NULL) {
         if (debug == 2) {
             ZF_LOGD("newline not found!\r\n");
         }
         return 0;
     }
     size_t length = (nlptr - &output_buffers[b][0]) + 1;
     if (length < output_buffers_used[b] && (output_buffers[b][length] == '\n' || output_buffers[b][length] == '\r')) {
         length++;               /* Include \n after \r if present */
     }
     if (length == 0) {
         if (debug == 2) {
             ZF_LOGD("0-length!\r\n");
         }
         return 0;
     }
     if (b != last_out) {
         printf("%s%s", COLOR_RESET, all_output_colours[COLOUR_INDEX_TO_STYLE(b)][COLOUR_INDEX_TO_SLOT(b)]);
         last_out = b;
     }
     int i;
     for (i = 0; i < length; i++) {
         printf("%c", output_buffers[b][i]);
     }
     for (i = length; i < output_buffers_used[b]; i++) {
         output_buffers[b][i - length] = output_buffers[b][i];
     }
     output_buffers_used[b] -= length;
     if (output_buffers_used[b] == 0) {
         output_buffer_bitmask &= ~BIT(b);
     }
     return 1;
 }

 static int is_newline(const uint8_t *c)
 {
     return (c[0] == '\r' && c[1] == '\n') || (c[0] == '\n' && c[1] == '\r');
 }

 static int active_client = 0;
 static int active_multiclients = 0;

 /* Try coalescing client output. This is intended for use with
  * multi-input mode to all clients. */

 /* (XXX) CAVEATS:
  *
  * - Has not been tested with more than 2 clients
  *
  * - Has not been tested with multi-input mode set not matching client
  *   set
  *
  * - Does not handle ANSI codes, UTF-8 or other multibytes specially;
      may break them when coalescing starts/stops.
  *
  * - Still "fails" due to some timing/buffering issues, but these
  *   failures are sufficiently rare that this is still useful. */
 static int try_coalesce_output()
 {
     size_t length = 0;
     size_t used[MAX_CLIENTS * 2] = { 0 };
     size_t n_used = 0;
     for (size_t i = 0; i < MAX_CLIENTS * 2; i++) {
         if (output_buffers_used[i] > 0) {
             used[n_used++] = i;
             if (length == 0 || length > output_buffers_used[i]) {
                 length = output_buffers_used[i];
             }
             if (n_used > 1) {
                 if (memcmp(output_buffers[used[0]], output_buffers[i], length) != 0) {
                     if (debug == 1) {
                         ZF_LOGD("\r\nDifferent contents '");
                         for (int j = 0; j < length; ++j) {
                             printf("%0hhx", output_buffers[used[0]][j]);
                         }
                         printf("' vs '");
                         for (int j = 0; j < length; ++j) {
                             printf("%0hhx", output_buffers[i][j]);
                         }
                         printf("'\r\n");
                     }
                     return -1; /* different contents, don't special-case */
                 }
             }
         }
     }
     if (n_used > 1 && length > 0) {
         if (last_out != -1) {
             printf("%s", COLOR_RESET);
         }
         for (int i = 0; i < length; i++) {
             printf("%c", output_buffers[used[0]][i]);
         }
         last_out = -1;
         fflush(stdout);
         for (int i = 0; i < n_used; i++) {
             output_buffers_used[used[i]] -= length;
             for (int j = 0; j < output_buffers_used[used[i]]; ++j) {
                 output_buffers[used[i]][j] = output_buffers[used[i]][j + length];
             }
             if (output_buffers_used[used[i]] == 0) {
                 output_buffer_bitmask &= ~BIT(used[i]);
             }
         }
         if (output_buffer_bitmask != 0) {
             has_data = 1;
         }
         return 0;               /* coalesced */
     }
     return 1;                   /* buffering */
 }

 static void internal_putchar(int b, int c)
 {
     int UNUSED error;
     error = serial_lock();
     /* Add to buffer */
     int index = output_buffers_used[b];
     uint8_t *buffer = output_buffers[b];
     buffer[index] = (uint8_t)c;
     output_buffers_used[b]++;
     int coalesce_status = -1;

     if (active_client == -1) {
         /* Test for special case: multiple clients outputting the EXACT SAME THING. */
         coalesce_status = try_coalesce_output();
     }
     if (output_buffers_used[b] == CLIENT_OUTPUT_BUFFER_SIZE) {
         /* Since we're violating contract anyway (flushing in the
          * middle of someone else's line), flush all buffers, so the
          * fastpath can be used again. */
         char is_done = 0;
         int i;
         int prev_client = last_out;
         if (prev_client != -1) {
             flush_buffer_line(prev_client);
         }
         while (!is_done) {
             is_done = 1;
             for (i = 0; i < MAX_CLIENTS * 2; i++) {
                 if (flush_buffer_line(i)) {
                     is_done = 0;
                 }
             }
         }
         /* Flush the rest, if necessary. */
         if (output_buffers_used[b] == CLIENT_OUTPUT_BUFFER_SIZE) {
             for (i = 0; i < MAX_CLIENTS * 2; i++) {
                 if (i != b) {
                     flush_buffer(i);
                 }
             }
         }
         /* then set the colors back. If this clients's buffer overflowed,
          * it's probably going to overflow again, so let's avoid
          * that. */
         if (last_out != b) {
             printf("%s%s", COLOR_RESET, all_output_colours[COLOUR_INDEX_TO_STYLE(b)][COLOUR_INDEX_TO_SLOT(b)]);
             last_out = b;
         }
     } else if ((index >= 1 && is_newline(buffer + index - 1) && coalesce_status == -1)
                || (last_out == b && output_buffer_bitmask == 0 && coalesce_status == -1)) {
         /* Allow fast output (newline or same-as-last-client) if
          * multi-input is not enabled OR last coalescing attempt
          * failed due to a mismatch. This is important as client output
          * may be delayed; coalescing failure due to empty buffer
          * should lead to further buffering rather than early flush,
          * in case we can coalesce later. */
         flush_buffer(b);
     } else {
         output_buffer_bitmask |= BIT(b);
     }
     has_data = 1;
     error = serial_unlock();
 }

 static void internal_raw_putchar(int id, int c)
 {
     getchar_client_t *client = &getchar_clients[id];
     uint32_t next_tail = (client->buf->tail + 1) % sizeof(client->buf->buf);
     if (next_tail == client->buf->head) {
         /* full */
         return;
     }
     uint32_t last_read_head = client->buf->head;
     client->buf->buf[client->buf->tail] = (uint8_t)c;
     /* no synchronize in here as we assume TSO */
     client->buf->tail = next_tail;
     __sync_synchronize();
     if (last_read_head != client->last_head) {
         getchar_emit(client->client_id);
         client->last_head = last_read_head;
     }
 }

 static int statemachine = 1;

 static void give_client_char(uint8_t c)
 {
     if (active_client >= 0) {
         internal_raw_putchar(active_client, c);
     } else if (active_client == -1) {
         for (int i = 0; i < MAX_CLIENTS * 2; i++) {
             if ((active_multiclients & BIT(i)) == BIT(i)) {
                 internal_raw_putchar(i, c);
             }
         }
     }
 }

 static void handle_char(uint8_t c)
 {
     /* If there are no getchar clients, then we return immediately */
     if (!getchar_num_badges) {
         return;
     }

     /* some manually written state machine magic to detect switching of input direction */
     switch (statemachine) {
     case 0:
         if (c == '\r' || c == '\n') {
             statemachine = 1;
         }
         give_client_char(c);
         break;
     case 1:
         if (c == ESCAPE_CHAR) {
             statemachine = 2;
         } else {
             statemachine = 0;
             give_client_char(c);
         }
         break;
     case 2:
         switch (c) {
         case ESCAPE_CHAR:
             statemachine = 0;
             give_client_char(c);
             break;
         case 'm':
             statemachine = 3;
             active_multiclients = 0;
             active_client = -1;
             last_out = -1;
             printf(COLOR_RESET "\r\nMulti-client input to clients: ");
             fflush(stdout);
             break;
         case 'd':
             debug = (debug + 1) % 3;
             printf(COLOR_RESET "\r\nDebug: %i\r\n", debug);
             last_out = -1;
             statemachine = 1;
             break;
         case '?':
             last_out = -1;
             printf(COLOR_RESET "\r\n --- SerialServer help ---"
                    "\r\n Escape char: %c"
                    "\r\n 0 - %-2d switches input to that client"
                    "\r\n ?      shows this help"
                    "\r\n m      simultaneous multi-client input"
                    "\r\n d      switch between debugging modes"
                    "\r\n          0: no debugging"
                    "\r\n          1: debug multi-input mode output coalescing"
                    "\r\n          2: debug flush_buffer_line"
                    "\r\n", ESCAPE_CHAR, num_getchar_clients - 1);
             statemachine = 1;
             break;
         default:
             if (c >= '0' && c < '0' + num_getchar_clients) {
                 last_out = -1;
                 int client = c - '0';
                 printf(COLOR_RESET "\r\nSwitching input to %d\r\n", client);
                 active_client = client;
                 statemachine = 1;
             } else {
                 statemachine = 0;
                 give_client_char(ESCAPE_CHAR);
                 give_client_char(c);
             }
         }
         break;
     case 3:
         if (c >= '0' && c < '0' + num_getchar_clients) {
             printf(COLOR_RESET "%s%d", (active_multiclients != 0 ? "," : ""), (c - '0'));
             active_multiclients |= BIT(c - '0');
             last_out = -1;
             fflush(stdout);
         } else if (c == 'm' || c == 'M' || c == '\r' || c == '\n') {
             last_out = -1;
             printf(COLOR_RESET "\r\nSwitching input to multi-client. Output will be best-effort coalesced (colored white).\r\n");
             statemachine = 1;
         }
         break;
     }
 }

 static void timer_callback(void *data)
 {
     int UNUSED error;
     error = serial_lock();
     if (done_output) {
         done_output = 0;
     } else if (has_data) {
         /* flush everything if no writes since last callback */
         int i;
         char is_done = 0;
         char succeeded = 0;
         while (!is_done) {
             is_done = 1;
             for (i = 0; i < MAX_CLIENTS * 2; i++) {
                 if (flush_buffer_line(i)) {
                     succeeded = 1;
                     is_done = 0;
                 }
             }
         }
         if (!succeeded) {
             for (i = 0; i < MAX_CLIENTS * 2; i++) {
                 flush_buffer(i);
             }
         }
     }
     error = serial_unlock();
 }

 seL4_CPtr timeout_notification(void);

 int run(void)
 {
     seL4_CPtr notification = timeout_notification();
     while (1) {
         seL4_Wait(notification, NULL);
         timer_callback(NULL);
     }
     return 0;
 }

 void serial_server_irq_handle(void *data, ps_irq_acknowledge_fn_t acknowledge_fn, void *ack_data)
 {
     int error = serial_lock();
     ZF_LOGF_IF(error, "Failed to lock serial server");

     plat_serial_interrupt(handle_char);

     error = acknowledge_fn(ack_data);
     ZF_LOGF_IF(error, "Failed to acknowledge IRQ");

     error = serial_unlock();
     ZF_LOGF_IF(error, "Failed to unlock serial server");
 }

 void serial_putchar(int c)
 {
     plat_serial_putchar(c);
 }

 void pre_init(void)
 {
     int error;
     error = serial_lock();

     error = camkes_io_ops(&io_ops);
     ZF_LOGF_IF(error, "Failed to initialise IO ops");

     // Initialize the serial port
     plat_pre_init(&io_ops);
     set_putchar(serial_putchar);
     /* query what getchar clients exist */
     if (getchar_num_badges) {
         num_getchar_clients = getchar_num_badges();
         getchar_clients = calloc(num_getchar_clients, sizeof(getchar_client_t));
         for (int i = 0; i < num_getchar_clients; i++) {
             unsigned int badge = getchar_enumerate_badge(i);
             assert(badge <= num_getchar_clients);
             getchar_clients[i].client_id = badge;
             getchar_clients[i].buf = getchar_buf(badge);
             getchar_clients[i].last_head = -1;
         }
     }
     plat_post_init(&(io_ops.irq_ops));
     /* Start regular heartbeat of 500ms */
     timeout_periodic(0, 500000000);
     error = serial_unlock();
 }

 void post_init(void)
 {
     if (num_registered_virtqueue_channels > 0) {
         int res = virtqueue_init();
         if (res) {
             ZF_LOGE("Serial server does not support read and write virtqueues");
         }
     }
 }

 seL4_Word processed_putchar_get_sender_id(void) WEAK;
 void processed_putchar_putchar(int c)
 {
     seL4_Word n = processed_putchar_get_sender_id();
     if (c == '\n') {
         internal_putchar(n, '\r');
     }
     internal_putchar((int)n, c);
 }

 seL4_Word raw_putchar_get_sender_id(void) WEAK;
 void raw_putchar_putchar(int c)
 {
     seL4_Word n = raw_putchar_get_sender_id();
     internal_putchar((int)n + MAX_CLIENTS, c);
 }

 seL4_Word processed_batch_get_sender_id(void) WEAK;
 void processed_batch_batch(void)
 {
     seL4_Word n = processed_batch_get_sender_id();
     client_buffer_t *putchar_buf = processed_batch_buf(n);
     while (putchar_buf->head != putchar_buf->tail) {
         char ch = putchar_buf->buf[putchar_buf->head];
         if (ch == '\n') {
             internal_putchar(n, '\r');
         }
         internal_putchar((int)n, ch);
         putchar_buf->head = (putchar_buf->head + 1) % sizeof(putchar_buf->buf);
     }
 }

 seL4_Word raw_batch_get_sender_id(void) WEAK;
 void raw_batch_batch(void)
 {
     seL4_Word n = raw_batch_get_sender_id();
     client_buffer_t *putchar_buf = raw_batch_buf(n);
     while (putchar_buf->head != putchar_buf->tail) {
         char ch = putchar_buf->buf[putchar_buf->head];
         internal_putchar((int)n + MAX_CLIENTS, ch);
         putchar_buf->head = (putchar_buf->head + 1) % sizeof(putchar_buf->buf);
     }
 }

 /* We had to define at least one function in the getchar RPC procedure
  * so now we need to implement it even though it is not used */
 void getchar_foo(void)
 {
     assert(!"should not be reached");
 }
	/*
	* Copyright 2017, Data61, CSIRO (ABN 41 687 119 230)
	*
	* SPDX-License-Identifier: BSD-2-Clause
	*/

	#include <autoconf.h>
	#include <camkes.h>
	#include <stdio.h>
	#include <stdint.h>
	#include <string.h>

	#include <sel4/sel4.h>
	#include <utils/attribute.h>
	#include <utils/ansi.h>
	#include <camkes.h>
	#include <camkes/io.h>
	#include <camkes/irq.h>
	#include <camkes/virtqueue.h>
	#include <platsupport/chardev.h>
	#include <platsupport/irq.h>
	#include "serial.h"
	#include "plat.h"
	#include "server_virtqueue.h"

	#define ESCAPE_CHAR '@'
	#define MAX_CLIENTS 12
	#define CLIENT_OUTPUT_BUFFER_SIZE 4096

	/* TODO: have the MultiSharedData template generate a header with these */
	void getchar_emit(unsigned int id) WEAK;
	seL4_Word getchar_enumerate_badge(unsigned int id) WEAK;
	unsigned int getchar_num_badges() WEAK;
	void *getchar_buf(unsigned int id) WEAK;
	void *processed_batch_buf(unsigned int id) WEAK;
	void *raw_batch_buf(unsigned int id) WEAK;
	int getchar_largest_badge(void) WEAK;

	typedef struct getchar_buffer {
	uint32_t head;
	uint32_t tail;
	char buf[4096 - 8];
	} client_buffer_t;

	compile_time_assert(getchar_buf_sized, sizeof(client_buffer_t) == sizeof(Buf));

	typedef struct getchar_client {
	unsigned int client_id;
	volatile client_buffer_t *buf;
	uint32_t last_head;
	} getchar_client_t;

	static ps_io_ops_t io_ops;

	static int last_out = -1;

	static uint8_t output_buffers[MAX_CLIENTS * 2][CLIENT_OUTPUT_BUFFER_SIZE];
	static int output_buffers_used[MAX_CLIENTS * 2] = { 0 };
	static uint16_t output_buffer_bitmask = 0;

	static int done_output = 0;

	static int has_data = 0;

	static int num_getchar_clients = 0;
	static getchar_client_t *getchar_clients = NULL;

	/* We predefine output colours for clients */
	const char *all_output_colours[2][MAX_CLIENTS] = {
	{
	/* Processed streams */
	ANSI_COLOR(RED),
	ANSI_COLOR(GREEN),
	ANSI_COLOR(BLUE),
	ANSI_COLOR(MAGENTA),
	ANSI_COLOR(YELLOW),
	ANSI_COLOR(CYAN),
	ANSI_COLOR(RED, BOLD)
	ANSI_COLOR(GREEN, BOLD),
	ANSI_COLOR(BLUE, BOLD),
	ANSI_COLOR(MAGENTA, BOLD),
	ANSI_COLOR(YELLOW, BOLD),
	ANSI_COLOR(CYAN, BOLD),
	},
	{
	/* Raw streams */
	ANSI_COLOR2(RED, WHITE),
	ANSI_COLOR2(GREEN, WHITE),
	ANSI_COLOR2(BLUE, WHITE),
	ANSI_COLOR2(MAGENTA, WHITE),
	ANSI_COLOR2(YELLOW, WHITE),
	ANSI_COLOR2(CYAN, WHITE),
	ANSI_COLOR2(RED, WHITE, BOLD)
	ANSI_COLOR2(GREEN, WHITE, BOLD),
	ANSI_COLOR2(BLUE, WHITE, BOLD),
	ANSI_COLOR2(MAGENTA, WHITE, BOLD),
	ANSI_COLOR2(YELLOW, WHITE, BOLD),
	ANSI_COLOR2(CYAN, WHITE, BOLD),
	},
	};

	#define COLOUR_INDEX_TO_STYLE(x) ((x) / (MAX_CLIENTS - 1))
	#define COLOUR_INDEX_TO_SLOT(x) ((x) % MAX_CLIENTS)

	static void flush_buffer(int b)
	{
	const char *col = all_output_colours[COLOUR_INDEX_TO_STYLE(b)][COLOUR_INDEX_TO_SLOT(b)];
	int i;
	if (output_buffers_used[b] == 0) {
	return;
	}
	if (b != last_out) {
	printf("%s%s", COLOR_RESET, col);
	last_out = b;
	}
	for (i = 0; i < output_buffers_used[b]; i++) {
	printf("%c", output_buffers[b][i]);
	}
	done_output = 1;
	output_buffers_used[b] = 0;
	output_buffer_bitmask &= ~BIT(b);
	fflush(stdout);
	}

	static int debug = 0;

	/* Try to flush up to the end of the line. */
	static bool flush_buffer_line(int b)
	{
	if (output_buffers_used[b] == 0) {
	return 0;
	}
	uint8_t *nlptr = memchr(output_buffers[b], '\r', output_buffers_used[b]);
	if (nlptr == NULL) {
	nlptr = memchr(output_buffers[b], '\n', output_buffers_used[b]);
	}
	if (nlptr == NULL) {
	if (debug == 2) {
	ZF_LOGD("newline not found!\r\n");
	}
	return 0;
	}
	size_t length = (nlptr - &output_buffers[b][0]) + 1;
	if (length < output_buffers_used[b] && (output_buffers[b][length] == '\n' \|\| output_buffers[b][length] == '\r')) {
	length++; /* Include \n after \r if present */
	}
	if (length == 0) {
	if (debug == 2) {
	ZF_LOGD("0-length!\r\n");
	}
	return 0;
	}
	if (b != last_out) {
	printf("%s%s", COLOR_RESET, all_output_colours[COLOUR_INDEX_TO_STYLE(b)][COLOUR_INDEX_TO_SLOT(b)]);
	last_out = b;
	}
	int i;
	for (i = 0; i < length; i++) {
	printf("%c", output_buffers[b][i]);
	}
	for (i = length; i < output_buffers_used[b]; i++) {
	output_buffers[b][i - length] = output_buffers[b][i];
	}
	output_buffers_used[b] -= length;
	if (output_buffers_used[b] == 0) {
	output_buffer_bitmask &= ~BIT(b);
	}
	return 1;
	}

	static int is_newline(const uint8_t *c)
	{
	return (c[0] == '\r' && c[1] == '\n') \|\| (c[0] == '\n' && c[1] == '\r');
	}

	static int active_client = 0;
	static int active_multiclients = 0;

	/* Try coalescing client output. This is intended for use with
	* multi-input mode to all clients. */

	/* (XXX) CAVEATS:
	*
	* - Has not been tested with more than 2 clients
	*
	* - Has not been tested with multi-input mode set not matching client
	* set
	*
	* - Does not handle ANSI codes, UTF-8 or other multibytes specially;
	may break them when coalescing starts/stops.
	*
	* - Still "fails" due to some timing/buffering issues, but these
	* failures are sufficiently rare that this is still useful. */
	static int try_coalesce_output()
	{
	size_t length = 0;
	size_t used[MAX_CLIENTS * 2] = { 0 };
	size_t n_used = 0;
	for (size_t i = 0; i < MAX_CLIENTS * 2; i++) {
	if (output_buffers_used[i] > 0) {
	used[n_used++] = i;
	if (length == 0 \|\| length > output_buffers_used[i]) {
	length = output_buffers_used[i];
	}
	if (n_used > 1) {
	if (memcmp(output_buffers[used[0]], output_buffers[i], length) != 0) {
	if (debug == 1) {
	ZF_LOGD("\r\nDifferent contents '");
	for (int j = 0; j < length; ++j) {
	printf("%0hhx", output_buffers[used[0]][j]);
	}
	printf("' vs '");
	for (int j = 0; j < length; ++j) {
	printf("%0hhx", output_buffers[i][j]);
	}
	printf("'\r\n");
	}
	return -1; /* different contents, don't special-case */
	}
	}
	}
	}
	if (n_used > 1 && length > 0) {
	if (last_out != -1) {
	printf("%s", COLOR_RESET);
	}
	for (int i = 0; i < length; i++) {
	printf("%c", output_buffers[used[0]][i]);
	}
	last_out = -1;
	fflush(stdout);
	for (int i = 0; i < n_used; i++) {
	output_buffers_used[used[i]] -= length;
	for (int j = 0; j < output_buffers_used[used[i]]; ++j) {
	output_buffers[used[i]][j] = output_buffers[used[i]][j + length];
	}
	if (output_buffers_used[used[i]] == 0) {
	output_buffer_bitmask &= ~BIT(used[i]);
	}
	}
	if (output_buffer_bitmask != 0) {
	has_data = 1;
	}
	return 0; /* coalesced */
	}
	return 1; /* buffering */
	}

	static void internal_putchar(int b, int c)
	{
	int UNUSED error;
	error = serial_lock();
	/* Add to buffer */
	int index = output_buffers_used[b];
	uint8_t *buffer = output_buffers[b];
	buffer[index] = (uint8_t)c;
	output_buffers_used[b]++;
	int coalesce_status = -1;

	if (active_client == -1) {
	/* Test for special case: multiple clients outputting the EXACT SAME THING. */
	coalesce_status = try_coalesce_output();
	}
	if (output_buffers_used[b] == CLIENT_OUTPUT_BUFFER_SIZE) {
	/* Since we're violating contract anyway (flushing in the
	* middle of someone else's line), flush all buffers, so the
	* fastpath can be used again. */
	char is_done = 0;
	int i;
	int prev_client = last_out;
	if (prev_client != -1) {
	flush_buffer_line(prev_client);
	}
	while (!is_done) {
	is_done = 1;
	for (i = 0; i < MAX_CLIENTS * 2; i++) {
	if (flush_buffer_line(i)) {
	is_done = 0;
	}
	}
	}
	/* Flush the rest, if necessary. */
	if (output_buffers_used[b] == CLIENT_OUTPUT_BUFFER_SIZE) {
	for (i = 0; i < MAX_CLIENTS * 2; i++) {
	if (i != b) {
	flush_buffer(i);
	}
	}
	}
	/* then set the colors back. If this clients's buffer overflowed,
	* it's probably going to overflow again, so let's avoid
	* that. */
	if (last_out != b) {
	printf("%s%s", COLOR_RESET, all_output_colours[COLOUR_INDEX_TO_STYLE(b)][COLOUR_INDEX_TO_SLOT(b)]);
	last_out = b;
	}
	} else if ((index >= 1 && is_newline(buffer + index - 1) && coalesce_status == -1)
	\|\| (last_out == b && output_buffer_bitmask == 0 && coalesce_status == -1)) {
	/* Allow fast output (newline or same-as-last-client) if
	* multi-input is not enabled OR last coalescing attempt
	* failed due to a mismatch. This is important as client output
	* may be delayed; coalescing failure due to empty buffer
	* should lead to further buffering rather than early flush,
	* in case we can coalesce later. */
	flush_buffer(b);
	} else {
	output_buffer_bitmask \|= BIT(b);
	}
	has_data = 1;
	error = serial_unlock();
	}

	static void internal_raw_putchar(int id, int c)
	{
	getchar_client_t *client = &getchar_clients[id];
	uint32_t next_tail = (client->buf->tail + 1) % sizeof(client->buf->buf);
	if (next_tail == client->buf->head) {
	/* full */
	return;
	}
	uint32_t last_read_head = client->buf->head;
	client->buf->buf[client->buf->tail] = (uint8_t)c;
	/* no synchronize in here as we assume TSO */
	client->buf->tail = next_tail;
	__sync_synchronize();
	if (last_read_head != client->last_head) {
	getchar_emit(client->client_id);
	client->last_head = last_read_head;
	}
	}

	static int statemachine = 1;

	static void give_client_char(uint8_t c)
	{
	if (active_client >= 0) {
	internal_raw_putchar(active_client, c);
	} else if (active_client == -1) {
	for (int i = 0; i < MAX_CLIENTS * 2; i++) {
	if ((active_multiclients & BIT(i)) == BIT(i)) {
	internal_raw_putchar(i, c);
	}
	}
	}
	}

	static void handle_char(uint8_t c)
	{
	/* If there are no getchar clients, then we return immediately */
	if (!getchar_num_badges) {
	return;
	}

	/* some manually written state machine magic to detect switching of input direction */
	switch (statemachine) {
	case 0:
	if (c == '\r' \|\| c == '\n') {
	statemachine = 1;
	}
	give_client_char(c);
	break;
	case 1:
	if (c == ESCAPE_CHAR) {
	statemachine = 2;
	} else {
	statemachine = 0;
	give_client_char(c);
	}
	break;
	case 2:
	switch (c) {
	case ESCAPE_CHAR:
	statemachine = 0;
	give_client_char(c);
	break;
	case 'm':
	statemachine = 3;
	active_multiclients = 0;
	active_client = -1;
	last_out = -1;
	printf(COLOR_RESET "\r\nMulti-client input to clients: ");
	fflush(stdout);
	break;
	case 'd':
	debug = (debug + 1) % 3;
	printf(COLOR_RESET "\r\nDebug: %i\r\n", debug);
	last_out = -1;
	statemachine = 1;
	break;
	case '?':
	last_out = -1;
	printf(COLOR_RESET "\r\n --- SerialServer help ---"
	"\r\n Escape char: %c"
	"\r\n 0 - %-2d switches input to that client"
	"\r\n ? shows this help"
	"\r\n m simultaneous multi-client input"
	"\r\n d switch between debugging modes"
	"\r\n 0: no debugging"
	"\r\n 1: debug multi-input mode output coalescing"
	"\r\n 2: debug flush_buffer_line"
	"\r\n", ESCAPE_CHAR, num_getchar_clients - 1);
	statemachine = 1;
	break;
	default:
	if (c >= '0' && c < '0' + num_getchar_clients) {
	last_out = -1;
	int client = c - '0';
	printf(COLOR_RESET "\r\nSwitching input to %d\r\n", client);
	active_client = client;
	statemachine = 1;
	} else {
	statemachine = 0;
	give_client_char(ESCAPE_CHAR);
	give_client_char(c);
	}
	}
	break;
	case 3:
	if (c >= '0' && c < '0' + num_getchar_clients) {
	printf(COLOR_RESET "%s%d", (active_multiclients != 0 ? "," : ""), (c - '0'));
	active_multiclients \|= BIT(c - '0');
	last_out = -1;
	fflush(stdout);
	} else if (c == 'm' \|\| c == 'M' \|\| c == '\r' \|\| c == '\n') {
	last_out = -1;
	printf(COLOR_RESET "\r\nSwitching input to multi-client. Output will be best-effort coalesced (colored white).\r\n");
	statemachine = 1;
	}
	break;
	}
	}

	static void timer_callback(void *data)
	{
	int UNUSED error;
	error = serial_lock();
	if (done_output) {
	done_output = 0;
	} else if (has_data) {
	/* flush everything if no writes since last callback */
	int i;
	char is_done = 0;
	char succeeded = 0;
	while (!is_done) {
	is_done = 1;
	for (i = 0; i < MAX_CLIENTS * 2; i++) {
	if (flush_buffer_line(i)) {
	succeeded = 1;
	is_done = 0;
	}
	}
	}
	if (!succeeded) {
	for (i = 0; i < MAX_CLIENTS * 2; i++) {
	flush_buffer(i);
	}
	}
	}
	error = serial_unlock();
	}

	seL4_CPtr timeout_notification(void);

	int run(void)
	{
	seL4_CPtr notification = timeout_notification();
	while (1) {
	seL4_Wait(notification, NULL);
	timer_callback(NULL);
	}
	return 0;
	}

	void serial_server_irq_handle(void data, ps_irq_acknowledge_fn_t acknowledge_fn, void ack_data)
	{
	int error = serial_lock();
	ZF_LOGF_IF(error, "Failed to lock serial server");

	plat_serial_interrupt(handle_char);

	error = acknowledge_fn(ack_data);
	ZF_LOGF_IF(error, "Failed to acknowledge IRQ");

	error = serial_unlock();
	ZF_LOGF_IF(error, "Failed to unlock serial server");
	}

	void serial_putchar(int c)
	{
	plat_serial_putchar(c);
	}

	void pre_init(void)
	{
	int error;
	error = serial_lock();

	error = camkes_io_ops(&io_ops);
	ZF_LOGF_IF(error, "Failed to initialise IO ops");

	// Initialize the serial port
	plat_pre_init(&io_ops);
	set_putchar(serial_putchar);
	/* query what getchar clients exist */
	if (getchar_num_badges) {
	num_getchar_clients = getchar_num_badges();
	getchar_clients = calloc(num_getchar_clients, sizeof(getchar_client_t));
	for (int i = 0; i < num_getchar_clients; i++) {
	unsigned int badge = getchar_enumerate_badge(i);
	assert(badge <= num_getchar_clients);
	getchar_clients[i].client_id = badge;
	getchar_clients[i].buf = getchar_buf(badge);
	getchar_clients[i].last_head = -1;
	}
	}
	plat_post_init(&(io_ops.irq_ops));
	/* Start regular heartbeat of 500ms */
	timeout_periodic(0, 500000000);
	error = serial_unlock();
	}

	void post_init(void)
	{
	if (num_registered_virtqueue_channels > 0) {
	int res = virtqueue_init();
	if (res) {
	ZF_LOGE("Serial server does not support read and write virtqueues");
	}
	}
	}

	seL4_Word processed_putchar_get_sender_id(void) WEAK;
	void processed_putchar_putchar(int c)
	{
	seL4_Word n = processed_putchar_get_sender_id();
	if (c == '\n') {
	internal_putchar(n, '\r');
	}
	internal_putchar((int)n, c);
	}

	seL4_Word raw_putchar_get_sender_id(void) WEAK;
	void raw_putchar_putchar(int c)
	{
	seL4_Word n = raw_putchar_get_sender_id();
	internal_putchar((int)n + MAX_CLIENTS, c);
	}

	seL4_Word processed_batch_get_sender_id(void) WEAK;
	void processed_batch_batch(void)
	{
	seL4_Word n = processed_batch_get_sender_id();
	client_buffer_t *putchar_buf = processed_batch_buf(n);
	while (putchar_buf->head != putchar_buf->tail) {
	char ch = putchar_buf->buf[putchar_buf->head];
	if (ch == '\n') {
	internal_putchar(n, '\r');
	}
	internal_putchar((int)n, ch);
	putchar_buf->head = (putchar_buf->head + 1) % sizeof(putchar_buf->buf);
	}
	}

	seL4_Word raw_batch_get_sender_id(void) WEAK;
	void raw_batch_batch(void)
	{
	seL4_Word n = raw_batch_get_sender_id();
	client_buffer_t *putchar_buf = raw_batch_buf(n);
	while (putchar_buf->head != putchar_buf->tail) {
	char ch = putchar_buf->buf[putchar_buf->head];
	internal_putchar((int)n + MAX_CLIENTS, ch);
	putchar_buf->head = (putchar_buf->head + 1) % sizeof(putchar_buf->buf);
	}
	}

	/* We had to define at least one function in the getchar RPC procedure
	* so now we need to implement it even though it is not used */
	void getchar_foo(void)
	{
	assert(!"should not be reached");
	}