Support for MPC5643L.

Rework of the start sequence to reduce the amount assembler code and to
support configuration tables which may be provided by the application.

4 files changed, 957 insertions, 0 deletions

diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-config.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-config.c
new file mode 100644
index 0000000000..816c8552b6
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-config.c
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp/console-generic.h>
+#include <bsp/console-esci.h>
+#include <bsp/console-linflex.h>
+
+CONSOLE_GENERIC_INFO_TABLE = {
+  #ifdef MPC55XX_HAS_ESCI
+    CONSOLE_GENERIC_INFO(mpc55xx_esci_devices + 0, &mpc55xx_esci_callbacks, "/dev/ttyS0"),
+    CONSOLE_GENERIC_INFO(mpc55xx_esci_devices + 1, &mpc55xx_esci_callbacks, "/dev/ttyS1")
+  #endif
+  #ifdef MPC55XX_HAS_LINFLEX
+    CONSOLE_GENERIC_INFO(mpc55xx_linflex_devices + 0, &mpc55xx_linflex_callbacks, "/dev/ttyS0"),
+    CONSOLE_GENERIC_INFO(mpc55xx_linflex_devices + 1, &mpc55xx_linflex_callbacks, "/dev/ttyS1")
+  #endif
+};
+
+CONSOLE_GENERIC_INFO_COUNT;
+
+CONSOLE_GENERIC_MINOR(0);
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-esci.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-esci.c
new file mode 100644
index 0000000000..737936b75f
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-esci.c
@@ -0,0 +1,339 @@
+/**
+ * @file
+ *
+ * @brief Console ESCI implementation.
+ */
+
+/*
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp/console-esci.h>
+
+#include <bsp.h>
+#include <bsp/irq.h>
+
+#ifdef MPC55XX_HAS_ESCI
+
+mpc55xx_esci_context mpc55xx_esci_devices [] = {
+  {
+    .regs = &ESCI_A,
+    .irq = MPC55XX_IRQ_ESCI(0)
+  }, {
+    .regs = &ESCI_B,
+    .irq = MPC55XX_IRQ_ESCI(1)
+  }
+};
+
+static void mpc55xx_esci_poll_write(int minor, char c)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  const union ESCI_SR_tag clear_tdre = { .B = { .TDRE = 1 } };
+  volatile struct ESCI_tag *regs = self->regs;
+  rtems_interrupt_level level;
+  bool done = false;
+  bool wait_for_transmit_done = false;
+
+  rtems_interrupt_disable(level);
+  if (self->transmit_nest_level == 0) {
+    union ESCI_CR1_tag cr1 = { .R = regs->CR1.R };
+
+    if (cr1.B.TIE != 0) {
+      cr1.B.TIE = 0;
+      regs->CR1.R = cr1.R;
+      wait_for_transmit_done = !self->transmit_in_progress;
+      self->transmit_nest_level = 1;
+    }
+  } else {
+    ++self->transmit_nest_level;
+  }
+  rtems_interrupt_enable(level);
+
+  while (!done) {
+    rtems_interrupt_disable(level);
+    bool tx = self->transmit_in_progress;
+    if (!tx || (tx && regs->SR.B.TDRE)) {
+      regs->SR.R = clear_tdre.R;
+      regs->DR.B.D = c;
+      self->transmit_in_progress = true;
+      done = true;
+    }
+    rtems_interrupt_enable(level);
+  }
+
+  done = false;
+  while (!done) {
+    rtems_interrupt_disable(level);
+    if (wait_for_transmit_done) {
+      if (regs->SR.B.TDRE) {
+        regs->SR.R = clear_tdre.R;
+        self->transmit_in_progress = false;
+        done = true;
+      }
+    } else {
+      done = true;
+    }
+
+    if (done && self->transmit_nest_level > 0) {
+      --self->transmit_nest_level;
+
+      if (self->transmit_nest_level == 0) {
+        union ESCI_CR1_tag cr1 = { .R = regs->CR1.R };
+
+        cr1.B.TIE = 1;
+        regs->CR1.R = cr1.R;
+      }
+    }
+    rtems_interrupt_enable(level);
+  }
+}
+
+static inline void mpc55xx_esci_interrupts_clear_and_enable(
+  mpc55xx_esci_context *self
+)
+{
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_CR1_tag cr1 = MPC55XX_ZERO_FLAGS;
+  rtems_interrupt_level level;
+
+  rtems_interrupt_disable(level);
+  cr1.R = regs->CR1.R;
+  cr1.B.RIE = 1;
+  cr1.B.TIE = 1;
+  regs->CR1.R = cr1.R;
+  regs->SR.R = regs->SR.R;
+  rtems_interrupt_enable(level);
+}
+
+static inline void mpc55xx_esci_interrupts_disable(mpc55xx_esci_context *self)
+{
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_CR1_tag cr1 = MPC55XX_ZERO_FLAGS;
+  rtems_interrupt_level level;
+
+  rtems_interrupt_disable(level);
+  cr1.R = regs->CR1.R;
+  cr1.B.RIE = 0;
+  cr1.B.TIE = 0;
+  regs->CR1.R = cr1.R;
+  rtems_interrupt_enable(level);
+}
+
+static void mpc55xx_esci_interrupt_handler(void *arg)
+{
+  mpc55xx_esci_context *self = arg;
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_SR_tag sr = MPC55XX_ZERO_FLAGS;
+  union ESCI_SR_tag active = MPC55XX_ZERO_FLAGS;
+  rtems_interrupt_level level;
+
+  /* Status */
+  sr.R = regs->SR.R;
+
+  /* Receive data register full? */
+  if (sr.B.RDRF != 0) {
+    active.B.RDRF = 1;
+  }
+
+  /* Transmit data register empty? */
+  if (sr.B.TDRE != 0) {
+    active.B.TDRE = 1;
+  }
+
+  /* Clear flags */
+  rtems_interrupt_disable(level);
+  regs->SR.R = active.R;
+  self->transmit_in_progress = false;
+  rtems_interrupt_enable(level);
+
+  /* Enqueue */
+  if (active.B.RDRF != 0) {
+    char c = regs->DR.B.D;
+    rtems_termios_enqueue_raw_characters(self->tty, &c, 1);
+  }
+
+  /* Dequeue */
+  if (active.B.TDRE != 0) {
+    rtems_termios_dequeue_characters(self->tty, 1);
+  }
+}
+
+static int mpc55xx_esci_set_attributes(int minor, const struct termios *t)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_CR1_tag cr1 = { .R = regs->CR1.R };
+  union ESCI_CR2_tag cr2 = MPC55XX_ZERO_FLAGS;
+  rtems_termios_baud_t br = rtems_termios_baud_to_number(t->c_cflag);
+
+  /* Enable module */
+  cr2.B.MDIS = 0;
+
+  /* Interrupts */
+  cr1.B.TCIE = 0;
+  cr1.B.ILIE = 0;
+  cr2.B.IEBERR = 0;
+  cr2.B.ORIE = 0;
+  cr2.B.NFIE = 0;
+  cr2.B.FEIE = 0;
+  cr2.B.PFIE = 0;
+
+  /* Disable receiver wake-up standby */
+  cr1.B.RWU = 0;
+
+  /* Disable DMA channels */
+  cr2.B.RXDMA = 0;
+  cr2.B.TXDMA = 0;
+
+  /* Idle line type */
+  cr1.B.ILT = 0;
+
+  /* Disable loops */
+  cr1.B.LOOPS = 0;
+
+  /* Enable or disable receiver */
+  cr1.B.RE = (t->c_cflag & CREAD) ? 1 : 0;
+
+  /* Enable transmitter */
+  cr1.B.TE = 1;
+
+  /* Baud rate */
+  if (br > 0) {
+    br = bsp_clock_speed / (16 * br);
+    br = (br > 8191) ? 8191 : br;
+  } else {
+    br = 0;
+  }
+  cr1.B.SBR = br;
+
+  /* Number of data bits */
+  if ((t->c_cflag & CSIZE) != CS8) {
+    return -1;
+  }
+  cr1.B.M = 0;
+
+  /* Parity */
+  cr1.B.PE = (t->c_cflag & PARENB) ? 1 : 0;
+  cr1.B.PT = (t->c_cflag & PARODD) ? 1 : 0;
+
+  /* Stop bits */
+  if (t->c_cflag & CSTOPB ) {
+    /* Two stop bits */
+    return -1;
+  }
+
+  /* Disable LIN */
+  regs->LCR.R = 0;
+
+  /* Set control registers */
+  regs->CR2.R = cr2.R;
+  regs->CR1.R = cr1.R;
+
+  return 0;
+}
+
+static int mpc55xx_esci_first_open(int major, int minor, void *arg)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  int rv = 0;
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  struct rtems_termios_tty *tty = console_generic_get_tty_at_open(arg);
+
+  self->tty = tty;
+
+  rv = rtems_termios_set_initial_baud(tty, 115200);
+  if (rv != 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  rv = mpc55xx_esci_set_attributes(minor, &tty->termios);
+  if (rv != 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  sc = mpc55xx_interrupt_handler_install(
+    self->irq,
+    "eSCI",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_DEFAULT_PRIORITY,
+    mpc55xx_esci_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  mpc55xx_esci_interrupts_clear_and_enable(self);
+  self->transmit_in_progress = false;
+
+  return 0;
+}
+
+static int mpc55xx_esci_last_close(int major, int minor, void* arg)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+
+  mpc55xx_esci_interrupts_disable(self);
+  self->tty = NULL;
+
+  return 0;
+}
+
+static int mpc55xx_esci_poll_read(int minor)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_SR_tag sr = MPC55XX_ZERO_FLAGS;
+  rtems_interrupt_level level;
+  int c = -1;
+
+  rtems_interrupt_disable(level);
+  if (regs->SR.B.RDRF != 0) {
+    /* Clear flag */
+    sr.B.RDRF = 1;
+    regs->SR.R = sr.R;
+
+    /* Read */
+    c = regs->DR.B.D;
+  }
+  rtems_interrupt_enable(level);
+
+  return c;
+}
+
+static int mpc55xx_esci_write(int minor, const char *out, size_t n)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  rtems_interrupt_level level;
+
+  rtems_interrupt_disable(level);
+  self->regs->DR.B.D = out [0];
+  self->transmit_in_progress = true;
+  rtems_interrupt_enable(level);
+
+  return 0;
+}
+
+const console_generic_callbacks mpc55xx_esci_callbacks = {
+  .termios_callbacks = {
+    .firstOpen = mpc55xx_esci_first_open,
+    .lastClose = mpc55xx_esci_last_close,
+    .write = mpc55xx_esci_write,
+    .setAttributes = mpc55xx_esci_set_attributes,
+    .outputUsesInterrupts = TERMIOS_IRQ_DRIVEN
+  },
+  .poll_read = mpc55xx_esci_poll_read,
+  .poll_write = mpc55xx_esci_poll_write
+};
+
+#endif /* MPC55XX_HAS_ESCI */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-generic.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-generic.c
new file mode 100644
index 0000000000..e6c2c587a2
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-generic.c
@@ -0,0 +1,167 @@
+/**
+ * @file
+ *
+ * @brief Generic console driver implementation.
+ */
+
+/*
+ * Copyright (c) 2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp/console-generic.h>
+
+#include <rtems/console.h>
+
+static const struct termios console_generic_termios = {
+  .c_cflag = CS8 | CREAD | CLOCAL | B115200
+};
+
+static void console_generic_char_out(char c)
+{
+  int minor = (int) console_generic_minor;
+  const console_generic_callbacks *cb =
+    console_generic_info_table [minor].callbacks;
+
+  if (c == '\n') {
+    (*cb->poll_write)(minor, '\r');
+  }
+
+  (*cb->poll_write)(minor, c);
+}
+
+static int console_generic_char_in(void)
+{
+  int minor = (int) console_generic_minor;
+  const console_generic_callbacks *cb =
+    console_generic_info_table [minor].callbacks;
+
+  return (*cb->poll_read)(minor);
+}
+
+static void console_generic_char_out_do_init(void)
+{
+  int minor = (int) console_generic_minor;
+  const console_generic_callbacks *cb =
+    console_generic_info_table [minor].callbacks;
+  const struct termios *term = &console_generic_termios;
+
+  BSP_output_char = console_generic_char_out;
+  (*cb->termios_callbacks.setAttributes)(minor, term);
+}
+
+static void console_generic_char_out_init(char c)
+{
+  console_generic_char_out_do_init();
+  console_generic_char_out(c);
+}
+
+rtems_device_driver console_initialize(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  const console_generic_info *info_table = console_generic_info_table;
+  rtems_device_minor_number count = console_generic_info_count;
+  rtems_device_minor_number console = console_generic_minor;
+
+  if (count <= 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  rtems_termios_initialize();
+
+  for (minor = 0; minor < count; ++minor) {
+    const console_generic_info *info = info_table + minor;
+
+    sc = rtems_io_register_name(info->device_path, major, minor);
+    if (sc != RTEMS_SUCCESSFUL) {
+      rtems_fatal_error_occurred(0xdeadbeef);
+    }
+  }
+
+  sc = rtems_io_register_name(CONSOLE_DEVICE_NAME, major, console);
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  console_generic_char_out_do_init();
+
+  return sc;
+}
+
+rtems_device_driver console_open(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  rtems_device_minor_number count = console_generic_info_count;
+
+  if (minor < count) {
+    const console_generic_info *info = &console_generic_info_table [minor];
+
+    sc = rtems_termios_open(
+      major,
+      minor,
+      arg,
+      &info->callbacks->termios_callbacks
+    );
+  } else {
+    sc = RTEMS_INVALID_ID;
+  }
+
+  return sc;
+}
+
+rtems_device_driver console_close(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  return rtems_termios_close(arg);
+}
+
+rtems_device_driver console_read(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  return rtems_termios_read(arg);
+}
+
+rtems_device_driver console_write(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  return rtems_termios_write(arg);
+}
+
+rtems_device_driver console_control(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  return rtems_termios_ioctl(arg);
+}
+
+BSP_output_char_function_type BSP_output_char = console_generic_char_out_init;
+
+BSP_polling_getchar_function_type BSP_poll_char = console_generic_char_in;
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-linflex.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-linflex.c
new file mode 100644
index 0000000000..54ac08ed56
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-linflex.c
@@ -0,0 +1,419 @@
+/**
+ * @file
+ *
+ * @brief Console LINFlexD implementation.
+ */
+
+/*
+ * Copyright (c) 2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp/console-linflex.h>
+
+#include <bsp.h>
+#include <bsp/irq.h>
+
+#ifdef MPC55XX_HAS_LINFLEX
+
+mpc55xx_linflex_context mpc55xx_linflex_devices [] = {
+  {
+    .regs = &LINFLEX0,
+    .irq_rxi = MPC55XX_IRQ_LINFLEX_RXI(0),
+    .irq_txi = MPC55XX_IRQ_LINFLEX_TXI(0),
+    .irq_err = MPC55XX_IRQ_LINFLEX_ERR(0),
+    .tx_pcr_register = &((SIU_tag *) &SIUL)->PCR18,
+    .tx_pa_value = 1,
+    .rx_pcr_register = &((SIU_tag *) &SIUL)->PCR19,
+    .rx_psmi_register = &((SIU_tag *) &SIUL)->PSMI31,
+    .rx_padsel_value = 0
+  }, {
+    .regs = &LINFLEX1,
+    .irq_rxi = MPC55XX_IRQ_LINFLEX_RXI(1),
+    .irq_txi = MPC55XX_IRQ_LINFLEX_TXI(1),
+    .irq_err = MPC55XX_IRQ_LINFLEX_ERR(1),
+    .tx_pcr_register = &((SIU_tag *) &SIUL)->PCR94,
+    .tx_pa_value = 1,
+    .rx_pcr_register = &((SIU_tag *) &SIUL)->PCR95,
+    .rx_psmi_register = &((SIU_tag *) &SIUL)->PSMI32,
+    .rx_padsel_value = 2
+  }
+};
+
+void enter_init_mode(volatile LINFLEX_tag *regs)
+{
+  LINFLEX_LINCR1_32B_tag cr1 = { .R = regs->LINCR1.R };
+  cr1.B.SLEEP = 0;
+  cr1.B.INIT = 1;
+  regs->LINCR1.R = cr1.R;
+}
+
+void enter_active_mode(volatile LINFLEX_tag *regs)
+{
+  LINFLEX_LINCR1_32B_tag cr1 = { .R = regs->LINCR1.R };
+  cr1.B.SLEEP = 0;
+  cr1.B.INIT = 0;
+  regs->LINCR1.R = cr1.R;
+}
+
+void enter_sleep_mode(volatile LINFLEX_tag *regs)
+{
+  LINFLEX_LINCR1_32B_tag cr1 = { .R = regs->LINCR1.R };
+  cr1.B.SLEEP = 1;
+  cr1.B.INIT = 0;
+  regs->LINCR1.R = cr1.R;
+}
+
+static void mpc55xx_linflex_poll_write(int minor, char c)
+{
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  const LINFLEX_UARTSR_32B_tag clear_dtf = { .B = { .DTF_TFF = 1 } };
+  rtems_interrupt_level level;
+  bool done = false;
+  bool wait_for_transmit_done = false;
+
+  rtems_interrupt_disable(level);
+  if (self->transmit_nest_level == 0) {
+	LINFLEX_LINIER_32B_tag ier = { .R = regs->LINIER.R };
+
+	if (ier.B.DTIE != 0) {
+	  ier.B.DTIE = 0;
+	  regs->LINIER.R = ier.R;
+	  wait_for_transmit_done = !self->transmit_in_progress;
+	  self->transmit_nest_level = 1;
+	}
+  } else {
+	++self->transmit_nest_level;
+  }
+  rtems_interrupt_enable(level);
+
+  while (!done) {
+	rtems_interrupt_disable(level);
+	bool tx = self->transmit_in_progress;
+	if (!tx || (tx && regs->UARTSR.B.DTF_TFF)) {
+	  regs->UARTSR.R = clear_dtf.R;
+	  regs->BDRL.B.DATA0 = c;
+	  self->transmit_in_progress = true;
+	  done = true;
+	}
+	rtems_interrupt_enable(level);
+  }
+
+  done = false;
+  while (!done) {
+	rtems_interrupt_disable(level);
+	if (wait_for_transmit_done) {
+	  if (regs->UARTSR.B.DTF_TFF) {
+		regs->UARTSR.R = clear_dtf.R;
+		self->transmit_in_progress = false;
+		done = true;
+	  }
+	} else {
+	  done = true;
+	}
+
+	if (done && self->transmit_nest_level > 0) {
+	  --self->transmit_nest_level;
+
+	  if (self->transmit_nest_level == 0) {
+        LINFLEX_LINIER_32B_tag ier = { .R = regs->LINIER.R };
+
+        ier.B.DTIE = 1;
+		regs->LINIER.R = ier.R;
+	  }
+	}
+	rtems_interrupt_enable(level);
+  }
+}
+
+static void mpc55xx_linflex_rx_interrupt_handler(void *arg)
+{
+  mpc55xx_linflex_context *self = arg;
+  volatile LINFLEX_tag *regs = self->regs;
+  char c = regs->BDRM.B.DATA4;
+  const LINFLEX_UARTSR_32B_tag clear_flags = { .B = { .RMB = 1, .DRF_RFE = 1 } };
+
+  regs->UARTSR.R = clear_flags.R;
+
+  rtems_termios_enqueue_raw_characters(self->tty, &c, 1);
+}
+
+static void mpc55xx_linflex_tx_interrupt_handler(void *arg)
+{
+  mpc55xx_linflex_context *self = arg;
+  volatile LINFLEX_tag *regs = self->regs;
+
+  regs->UARTSR.B.DTF_TFF = 1;	/* clear flag */
+  self->transmit_in_progress = false;
+
+  rtems_termios_dequeue_characters(self->tty, 1);
+}
+
+/*
+static void mpc55xx_linflex_err_interrupt_handler(void *arg)
+{
+  mpc55xx_linflex_context *self = arg;
+}
+*/
+
+static int mpc55xx_linflex_set_attributes(int minor, const struct termios *t)
+{
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  LINFLEX_UARTCR_32B_tag uartcr = { .R = 0 };
+  LINFLEX_GCR_32B_tag gcr = { .R = 0 };
+  LINFLEX_LINIER_32B_tag ier = { .R = 0 };
+  rtems_termios_baud_t br = rtems_termios_baud_to_number(t->c_cflag);
+  LINFLEX_LINFBRR_32B_tag fbrr = { .R = 0 };
+  LINFLEX_LINIBRR_32B_tag ibrr = { .R = 0 };
+
+  enter_init_mode(regs);
+
+  /* Set to UART-mode */
+  uartcr.B.UART = 1;
+  regs->UARTCR.R = uartcr.R;
+
+  /* Set to buffer mode with size 1 */
+  uartcr.B.TDFL_TFC = 0;
+  uartcr.B.RDFL_RFC0 = 0;
+  uartcr.B.RFBM = 0;
+  uartcr.B.TFBM = 0;
+
+  /* Enable receiver and transmitter */
+  uartcr.B.RXEN = 1;
+  uartcr.B.TXEN = 1;
+
+  /* Number of data bits */
+  uartcr.B.WL1 = 0;
+  if ((t->c_cflag & CSIZE) == CS8) {
+	uartcr.B.WL0 = 1;
+  } else if ((t->c_cflag & CSIZE) == CS7) {
+	uartcr.B.WL0 = 0;
+  } else {
+    return -1;
+  }
+
+  /* Parity */
+  uartcr.B.PCE = (t->c_cflag & PARENB) ? 1 : 0;
+  uartcr.B.PC1 = 0;
+  uartcr.B.PC0 = (t->c_cflag & PARODD) ? 1 : 0;
+
+  /* Stop bits */
+  gcr.B.STOP = (t->c_cflag & CSTOPB) ? 1 : 0;
+
+  /* Set control registers */
+  regs->UARTCR.R = uartcr.R;
+  regs->GCR.R = gcr.R;
+
+  /* Interrupts */
+  ier.B.DTIE = 1;
+  ier.B.DRIE = 1;
+  regs->LINIER.R = ier.R;
+
+  /* Baud rate */
+  if (br > 0) {
+	uint32_t lfdiv_mult_32 = bsp_clock_speed * 2 / br;
+	if((lfdiv_mult_32 & 0x1) != 0) {
+		++lfdiv_mult_32;
+	}
+	fbrr.B.FBR = (lfdiv_mult_32 >> 1) & 0xF;
+	ibrr.B.IBR = lfdiv_mult_32 >> 5;
+  } else {
+    return -1;
+  }
+  regs->LINFBRR.R = fbrr.R;
+  regs->LINIBRR.R = ibrr.R;
+
+  enter_active_mode(regs);
+
+  return 0;
+}
+
+static int mpc55xx_linflex_first_open(int major, int minor, void *arg)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  int rv = 0;
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  struct rtems_termios_tty *tty = console_generic_get_tty_at_open(arg);
+  SIU_PCR_tag pcr = { .R = 0 };
+  SIUL_PSMI_8B_tag psmi = { .R = 0 };
+
+  self->tty = tty;
+
+  pcr.B.IBE = 1;
+  self->rx_pcr_register->R = pcr.R;
+  psmi.B.PADSEL = self->rx_padsel_value;
+  self->rx_psmi_register->R = psmi.R;
+  pcr.R = 0;
+  pcr.B.OBE = 1;
+  pcr.B.PA = self->tx_pa_value;
+  self->tx_pcr_register->R = pcr.R;
+
+  rv = rtems_termios_set_initial_baud(tty, 115200);
+  if (rv != 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  rv = mpc55xx_linflex_set_attributes(minor, &tty->termios);
+  if (rv != 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  sc = mpc55xx_interrupt_handler_install(
+    self->irq_rxi,
+    "LINFlexD RXI",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_DEFAULT_PRIORITY,
+    mpc55xx_linflex_rx_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  sc = mpc55xx_interrupt_handler_install(
+    self->irq_txi,
+    "LINFlexD TXI",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_DEFAULT_PRIORITY,
+    mpc55xx_linflex_tx_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  /*
+  sc = mpc55xx_interrupt_handler_install(
+    self->irq_err,
+    "LINFlexD ERR",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_DEFAULT_PRIORITY,
+    mpc55xx_linflex_err_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+  */
+
+  return 0;
+}
+
+static int mpc55xx_linflex_last_close(int major, int minor, void* arg)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  SIU_PCR_tag pcr = { .R = 0 };
+  SIUL_PSMI_8B_tag psmi = { .R = 0 };
+
+  /* enter initialization mode */
+  enter_init_mode(regs);
+
+  /* disable interrupts */
+  regs->LINIER.R = 0;
+
+  /* set module to sleep mode */
+  enter_sleep_mode(regs);
+
+  sc = rtems_interrupt_handler_remove(
+    self->irq_rxi,
+    mpc55xx_linflex_rx_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  sc = rtems_interrupt_handler_remove(
+    self->irq_txi,
+    mpc55xx_linflex_tx_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  /*
+  sc = rtems_interrupt_handler_remove(
+    self->irq_err,
+    mpc55xx_linflex_err_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+  */
+
+  pcr.B.IBE = 1;
+  self->rx_pcr_register->R = pcr.R;
+  self->tx_pcr_register->R = pcr.R;
+  psmi.R = 0;
+  self->rx_psmi_register->R = psmi.R;
+
+  self->tty = NULL;
+
+  return 0;
+}
+
+static int mpc55xx_linflex_poll_read(int minor)
+{
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  rtems_interrupt_level level;
+  int c = -1;
+
+  rtems_interrupt_disable(level);
+  if (regs->UARTSR.B.DRF_RFE != 0) {
+    /* Clear flag */
+    regs->UARTSR.B.DRF_RFE = 1;
+
+    /* Read */
+    c = regs->BDRM.B.DATA4;
+  }
+  rtems_interrupt_enable(level);
+
+  return c;
+}
+
+static int mpc55xx_linflex_write(int minor, const char *out, size_t n)
+{
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  rtems_interrupt_level level;
+
+  rtems_interrupt_disable(level);
+
+  regs->BDRL.B.DATA0 = out [0];
+  self->transmit_in_progress = true;
+  /* TODO: send more then one byte */
+
+  rtems_interrupt_enable(level);
+
+  return 0;
+}
+
+const console_generic_callbacks mpc55xx_linflex_callbacks = {
+  .termios_callbacks = {
+    .firstOpen = mpc55xx_linflex_first_open,
+    .lastClose = mpc55xx_linflex_last_close,
+    .write = mpc55xx_linflex_write,
+    .setAttributes = mpc55xx_linflex_set_attributes,
+    .outputUsesInterrupts = TERMIOS_IRQ_DRIVEN
+  },
+  .poll_read = mpc55xx_linflex_poll_read,
+  .poll_write = mpc55xx_linflex_poll_write
+};
+
+#endif /* MPC55XX_HAS_LINFLEX */