--- a/packages/hal/powerpc/cogent/current/src/hal_diag.c
+++ b/packages/hal/powerpc/cogent/current/src/hal_diag.c
@@ -42,85 +42,560 @@
 //=============================================================================
 
 #include <pkgconf/hal.h>
-#include <pkgconf/hal_powerpc_cogent.h> // CYGHWR_HAL_POWERPC_COGENT_DIAG_PORT
 
 #include <cyg/hal/hal_diag.h>           // our header.
 
 #if defined(CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS)
-#include <cyg/hal/hal_stub.h>           // hal_output_gdb_string
+#include <cyg/hal/hal_stub.h>           // CYG_HAL_GDB_ENTER_CRITICAL_IO_REGION
 #endif
 
 #include <cyg/infra/cyg_type.h>         // base types, externC
 #include <cyg/hal/hal_io.h>             // IO macros
 #include <cyg/hal/hal_intr.h>           // Interrupt macros
 
-//-----------------------------------------------------------------------------
-// Select default diag channel to use
+#include <cyg/hal/hal_arch.h>           // SAVE/RESTORE GP
+#include <cyg/hal/hal_if.h>             // Calling-if API
+
+
+static void cyg_hal_plf_serial_init(void);
+static void cyg_hal_plf_lcd_init(void);
 
-//#define CYG_KERNEL_DIAG_ROMART
-//#define CYG_KERNEL_DIAG_LCD
-//#define CYG_KERNEL_DIAG_SERIAL
+void
+cyg_hal_plf_comms_init(void)
+{
+    static int initialized = 0;
+
+    if (initialized)
+        return;
 
-#if !defined(CYG_KERNEL_DIAG_SERIAL) && \
-    !defined(CYG_KERNEL_DIAG_LCD)    && \
-    !defined(CYG_KERNEL_DIAG_ROMART)
+    initialized = 1;
+
+    cyg_hal_plf_serial_init();
+    cyg_hal_plf_lcd_init();
+}
 
-#define CYG_KERNEL_DIAG_SERIAL
-
+#if 0
+#ifdef CYGSEM_HAL_ROM_MONITOR
+    // It's handy to have the LCD initialized at reset when using it
+    // for debugging output.
+    {
+        diag_write_string ("eCos ROM   " __TIME__ "\n");
+        diag_write_string (__DATE__ "\n");
+    }
+#endif
 #endif
 
-// Always use LCD when building a GDB stub ROM.
-#ifdef CYGSEM_HAL_ROM_MONITOR
-#undef CYG_KERNEL_DIAG_SERIAL
-#undef CYG_KERNEL_DIAG_ROMART
+//=============================================================================
+// Serial driver
+//=============================================================================
+
+//-----------------------------------------------------------------------------
+// There are two serial ports.
+#define CYG_DEV_SERIAL_BASE_A    0xe900047 // port A
+#define CYG_DEV_SERIAL_BASE_B    0xe900007 // port B
+
+//-----------------------------------------------------------------------------
+// Default baud rate is 38400
+#define CYG_DEV_SERIAL_RS232_T1_VALUE_B38400         0x00
+#define CYG_DEV_SERIAL_RS232_T2_VALUE_B38400         0x06
+
+//-----------------------------------------------------------------------------
+// Define the serial registers. The Cogent board is equipped with a 16552
+// serial chip.
+#define CYG_DEV_SERIAL_RBR   0x00  // receiver buffer register, read, dlab = 0
+#define CYG_DEV_SERIAL_THR   0x00 // transmitter holding register, write, dlab = 0
+#define CYG_DEV_SERIAL_DLL   0x00 // divisor latch (LS), read/write, dlab = 1
+#define CYG_DEV_SERIAL_IER   0x08 // interrupt enable register, read/write, dlab = 0
+#define CYG_DEV_SERIAL_DLM   0x08 // divisor latch (MS), read/write, dlab = 1
+#define CYG_DEV_SERIAL_IIR   0x10 // interrupt identification register, read, dlab = 0
+#define CYG_DEV_SERIAL_FCR   0x10 // fifo control register, write, dlab = 0
+#define CYG_DEV_SERIAL_AFR   0x10 // alternate function register, read/write, dlab = 1
+#define CYG_DEV_SERIAL_LCR   0x18 // line control register, read/write
+#define CYG_DEV_SERIAL_MCR   0x20
+#define CYG_DEV_SERIAL_MCR_A 0x20
+#define CYG_DEV_SERIAL_MCR_B 0x20
+#define CYG_DEV_SERIAL_LSR   0x28 // line status register, read
+#define CYG_DEV_SERIAL_MSR   0x30 // modem status register, read
+#define CYG_DEV_SERIAL_SCR   0x38 // scratch pad register
+
+// The interrupt enable register bits.
+#define SIO_IER_ERDAI   0x01            // enable received data available irq
+#define SIO_IER_ETHREI  0x02            // enable THR empty interrupt
+#define SIO_IER_ELSI    0x04            // enable receiver line status irq
+#define SIO_IER_EMSI    0x08            // enable modem status interrupt
+
+// The interrupt identification register bits.
+#define SIO_IIR_IP      0x01            // 0 if interrupt pending
+#define SIO_IIR_ID_MASK 0x0e            // mask for interrupt ID bits
+
+// The line status register bits.
+#define SIO_LSR_DR      0x01            // data ready
+#define SIO_LSR_OE      0x02            // overrun error
+#define SIO_LSR_PE      0x04            // parity error
+#define SIO_LSR_FE      0x08            // framing error
+#define SIO_LSR_BI      0x10            // break interrupt
+#define SIO_LSR_THRE    0x20            // transmitter holding register empty
+#define SIO_LSR_TEMT    0x40            // transmitter register empty
+#define SIO_LSR_ERR     0x80            // any error condition
+
+// The modem status register bits.
+#define SIO_MSR_DCTS  0x01              // delta clear to send
+#define SIO_MSR_DDSR  0x02              // delta data set ready
+#define SIO_MSR_TERI  0x04              // trailing edge ring indicator
+#define SIO_MSR_DDCD  0x08              // delta data carrier detect
+#define SIO_MSR_CTS   0x10              // clear to send
+#define SIO_MSR_DSR   0x20              // data set ready
+#define SIO_MSR_RI    0x40              // ring indicator
+#define SIO_MSR_DCD   0x80              // data carrier detect
+
+// The line control register bits.
+#define SIO_LCR_WLS0   0x01             // word length select bit 0
+#define SIO_LCR_WLS1   0x02             // word length select bit 1
+#define SIO_LCR_STB    0x04             // number of stop bits
+#define SIO_LCR_PEN    0x08             // parity enable
+#define SIO_LCR_EPS    0x10             // even parity select
+#define SIO_LCR_SP     0x20             // stick parity
+#define SIO_LCR_SB     0x40             // set break
+#define SIO_LCR_DLAB   0x80             // divisor latch access bit
+
+// The FIFO control register
+#define SIO_FCR_FCR0   0x01             // enable xmit and rcvr fifos
+#define SIO_FCR_FCR1   0x02             // clear RCVR FIFO
+#define SIO_FCR_FCR2   0x04             // clear XMIT FIFO
+
+
+static void
+init_serial_channel( cyg_uint8* base )
+{
+    cyg_uint8 lcr;
+
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_IER, 0);
+
+    // Disable and clear FIFOs (need to enable to clear).
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_FCR,
+                    (SIO_FCR_FCR0 | SIO_FCR_FCR1 | SIO_FCR_FCR2));
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_FCR, 0);
+
+    // 8-1-no parity.
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_LCR, SIO_LCR_WLS0 | SIO_LCR_WLS1);
+
+    // Set speed to 38400.
+    HAL_READ_UINT8(base+CYG_DEV_SERIAL_LCR, lcr);
+    lcr |= SIO_LCR_DLAB;
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_LCR, lcr);
+
+
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_DLL,
+                    CYG_DEV_SERIAL_RS232_T2_VALUE_B38400);
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_DLM,
+                    CYG_DEV_SERIAL_RS232_T1_VALUE_B38400);
+    lcr &= ~SIO_LCR_DLAB;
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_LCR, lcr);
+
+    {
+        // Special initialization for ST16C552 on CMA102
+        cyg_uint8 mcr;
+        
+        HAL_READ_UINT8(base+CYG_DEV_SERIAL_MCR_A, mcr);
+        mcr |= 8;
+        HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_MCR_A, mcr);
+
+        HAL_READ_UINT8(base+CYG_DEV_SERIAL_MCR_B, mcr);
+        mcr |= 8;
+        HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_MCR_B, mcr);
+    }
+
+    // Enable FIFOs (and clear them).
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_FCR,
+                    (SIO_FCR_FCR0 | SIO_FCR_FCR1 | SIO_FCR_FCR2));
+}
+
+cyg_uint8
+cyg_hal_plf_serial_getc(void* __ch_data)
+{
+    cyg_uint8* base = (cyg_uint8*)__ch_data;
+    cyg_uint8 c, lsr;
+    CYGARC_HAL_SAVE_GP();
+
+    do {
+        HAL_READ_UINT8(base+CYG_DEV_SERIAL_LSR, lsr);
+    } while ((lsr & SIO_LSR_DR) == 0);
+
+    HAL_READ_UINT8(base+CYG_DEV_SERIAL_RBR, c);
+
+    CYGARC_HAL_RESTORE_GP();
+    return c;
+}
+
+void
+cyg_hal_plf_serial_putc(void* __ch_data, cyg_uint8 c)
+{
+    cyg_uint8* base = (cyg_uint8*)__ch_data;
+    cyg_uint8 lsr;
+    CYGARC_HAL_SAVE_GP();
+
+    do {
+        HAL_READ_UINT8(base+CYG_DEV_SERIAL_LSR, lsr);
+    } while ((lsr & SIO_LSR_THRE) == 0);
+
+    HAL_WRITE_UINT8(base+CYG_DEV_SERIAL_THR, c);
+
+    // Hang around until the character has been safely sent.
+    do {
+        HAL_READ_UINT8(base+CYG_DEV_SERIAL_LSR, lsr);
+    } while ((lsr & SIO_LSR_THRE) == 0);
+
+    CYGARC_HAL_RESTORE_GP();
+}
+
+#if defined(CYGSEM_HAL_VIRTUAL_VECTOR_DIAG) \
+    || defined(CYGPRI_HAL_IMPLEMENTS_IF_SERVICES)
+
+static void
+cyg_hal_plf_serial_write(void* __ch_data, const cyg_uint8* __buf, 
+                         cyg_uint32 __len)
+{
+    CYGARC_HAL_SAVE_GP();
+
+    while(__len-- > 0)
+        cyg_hal_plf_serial_putc(__ch_data, *__buf++);
+
+    CYGARC_HAL_RESTORE_GP();
+}
+
+static void
+cyg_hal_plf_serial_read(void* __ch_data, cyg_uint8* __buf, cyg_uint32 __len)
+{
+    CYGARC_HAL_SAVE_GP();
+
+    while(__len-- > 0)
+        *__buf++ = cyg_hal_plf_serial_getc(__ch_data);
+
+    CYGARC_HAL_RESTORE_GP();
+}
+
+static int
+cyg_hal_plf_serial_control(void *__ch_data, __comm_control_cmd_t __func, ...)
+{
+    // Do nothing (yet).
+    return 0;
+}
+
+#endif // CYGSEM_HAL_VIRTUAL_VECTOR_DIAG || CYGPRI_HAL_IMPLEMENTS_IF_SERVICES
+
+static void
+cyg_hal_plf_serial_init(void)
+{
+    // Disable interrupts.
+    HAL_INTERRUPT_MASK(CYGNUM_HAL_INTERRUPT_SIU_IRQ1);
+
+    // Init channels
+    init_serial_channel((cyg_uint8*)CYG_DEV_SERIAL_BASE_A);
+    init_serial_channel((cyg_uint8*)CYG_DEV_SERIAL_BASE_B);
+
+#if defined(CYGSEM_HAL_VIRTUAL_VECTOR_DIAG) \
+    || defined(CYGPRI_HAL_IMPLEMENTS_IF_SERVICES)
+    {
+        // Setup procs in the vector table
+        hal_virtual_comm_table_t* comm;
+        int cur = CYGACC_CALL_IF_SET_CONSOLE_COMM()(CYGNUM_CALL_IF_SET_COMM_ID_QUERY_CURRENT);
+
+        // Set channel 0
+        CYGACC_CALL_IF_SET_CONSOLE_COMM()(0);
+        comm = CYGACC_CALL_IF_CONSOLE_PROCS();
+        CYGACC_COMM_IF_CH_DATA_SET(*comm, CYG_DEV_SERIAL_BASE_A);
+        CYGACC_COMM_IF_WRITE_SET(*comm, cyg_hal_plf_serial_write);
+        CYGACC_COMM_IF_READ_SET(*comm, cyg_hal_plf_serial_read);
+        CYGACC_COMM_IF_PUTC_SET(*comm, cyg_hal_plf_serial_putc);
+        CYGACC_COMM_IF_GETC_SET(*comm, cyg_hal_plf_serial_getc);
+        CYGACC_COMM_IF_CONTROL_SET(*comm, cyg_hal_plf_serial_control);
 
-#define CYG_KERNEL_DIAG_LCD
+        // Set channel 1
+        CYGACC_CALL_IF_SET_CONSOLE_COMM()(1);
+        comm = CYGACC_CALL_IF_CONSOLE_PROCS();
+        CYGACC_COMM_IF_CH_DATA_SET(*comm, CYG_DEV_SERIAL_BASE_B);
+        CYGACC_COMM_IF_WRITE_SET(*comm, cyg_hal_plf_serial_write);
+        CYGACC_COMM_IF_READ_SET(*comm, cyg_hal_plf_serial_read);
+        CYGACC_COMM_IF_PUTC_SET(*comm, cyg_hal_plf_serial_putc);
+        CYGACC_COMM_IF_GETC_SET(*comm, cyg_hal_plf_serial_getc);
+        CYGACC_COMM_IF_CONTROL_SET(*comm, cyg_hal_plf_serial_control);
+     
+        // Restore original console
+        CYGACC_CALL_IF_SET_CONSOLE_COMM()(cur);
+    }
 #endif
+}
 
+
+//=============================================================================
+// LCD driver
+//=============================================================================
+// FEMA 162B 16 character x 2 line LCD
+// base addresses and register offsets *
+
+#define MBD_BASE 0
+
+#define LCD_BASE                (MBD_BASE + 0xEB00007)
+
+#define LCD_DATA                0x00 // read/write lcd data
+#define LCD_STAT                0x08 // read lcd busy status
+#define LCD_CMD                 0x08 // write lcd command
+
+// status register bit definitions
+#define LCD_STAT_BUSY   0x80    // 1 = display busy
+#define LCD_STAT_ADD    0x7F    // bits 0-6 return current display address
+
+// command register definitions
+#define LCD_CMD_RST             0x01    // clear entire display and reset display address
+#define LCD_CMD_HOME            0x02    // reset display address and reset any shifting
+#define LCD_CMD_ECL             0x04    // move cursor left one position on next data write
+#define LCD_CMD_ESL             0x05    // shift display left one position on next data write
+#define LCD_CMD_ECR             0x06    // move cursor right one position on next data write
+#define LCD_CMD_ESR             0x07    // shift display right one position on next data write
+#define LCD_CMD_DOFF            0x08    // display off, cursor off, blinking off
+#define LCD_CMD_BL              0x09    // blink character at current cursor position
+#define LCD_CMD_CUR             0x0A    // enable cursor on
+#define LCD_CMD_DON             0x0C    // turn display on
+#define LCD_CMD_CL              0x10    // move cursor left one position
+#define LCD_CMD_SL              0x14    // shift display left one position
+#define LCD_CMD_CR              0x18    // move cursor right one position
+#define LCD_CMD_SR              0x1C    // shift display right one position
+#define LCD_CMD_MODE            0x38    // sets 8 bits, 2 lines, 5x7 characters
+#define LCD_CMD_ACG             0x40    // bits 0-5 sets the character generator address
+#define LCD_CMD_ADD             0x80    // bits 0-6 sets the display data address to line 1 +
+
+// LCD status values
+#define LCD_OK                  0x00
+#define LCD_ERR                 0x01
+
+#define LCD_LINE0       0x00
+#define LCD_LINE1       0x40
+
+#define LCD_LINE_LENGTH 16
+
+static char lcd_line0[LCD_LINE_LENGTH+1];
+static char lcd_line1[LCD_LINE_LENGTH+1];
+static char *lcd_line[2] = { lcd_line0, lcd_line1 };
+static int lcd_curline = 0;
+static int lcd_linepos = 0;
+
+
+static void lcd_dis(int add, char *s, cyg_uint8* base);
+
+static void
+init_lcd_channel(cyg_uint8* base)
+{
+    cyg_uint8 stat;
+    int i;
+    
+    // wait for not busy
+    // Note: It seems that the LCD isn't quite ready to process commands
+    // when it clears the BUSY flag. Reading the status address an extra
+    // time seems to give it enough breathing room.
+    do { HAL_READ_UINT8(base+LCD_STAT, stat); } while (stat & LCD_STAT_BUSY);
+    HAL_READ_UINT8(base+LCD_STAT, stat);
+
+    // configure the lcd for 8 bits/char, 2 lines
+    // and 5x7 dot matrix
+    HAL_WRITE_UINT8(base+LCD_CMD, LCD_CMD_MODE);
+
+    // wait for not busy
+    do { HAL_READ_UINT8(base+LCD_STAT, stat); } while (stat & LCD_STAT_BUSY);
+    HAL_READ_UINT8(base+LCD_STAT, stat);
+
+    // turn the LCD display on
+    HAL_WRITE_UINT8(base+LCD_CMD, LCD_CMD_DON);
+
+    lcd_curline = 0;
+    lcd_linepos = 0;
+
+    for( i = 0; i < LCD_LINE_LENGTH; i++ )
+        lcd_line[0][i] = lcd_line[1][i] = ' ';
+
+    lcd_line[0][LCD_LINE_LENGTH] = lcd_line[1][LCD_LINE_LENGTH] = 0;
+
+    lcd_dis(LCD_LINE0, lcd_line[0], base);
+    lcd_dis(LCD_LINE1, lcd_line[1], base);
+}
+
+// this routine writes the string to the LCD
+// display after setting the address to add
+static void
+lcd_dis(int add, char *s, cyg_uint8* base)
+{
+    cyg_uint8 stat;
+    int i;
+
+    // wait for not busy (see Note in hal_diag_init above)
+    do { HAL_READ_UINT8(base+LCD_STAT, stat); } while (stat & LCD_STAT_BUSY);
+    HAL_READ_UINT8(base+LCD_STAT, stat);
+
+    // write the address
+    HAL_WRITE_UINT8(base+LCD_CMD, (LCD_CMD_ADD + add));
+
+    // write the string out to the display stopping when we reach 0
+    for (i = 0; *s != '\0'; i++)
+    {
+        // wait for not busy
+        do { HAL_READ_UINT8(base+LCD_STAT, stat); } while (stat & LCD_STAT_BUSY);
+        HAL_READ_UINT8(base+LCD_STAT, stat);
+
+        // write the data
+        HAL_WRITE_UINT8(base+LCD_DATA, *s++);
+    }
+}
+
+void
+cyg_hal_plf_lcd_putc(void* __ch_data, cyg_uint8 c)
+{
+    cyg_uint8* base = (cyg_uint8*)__ch_data;
+    unsigned long __state;
+    int i;
+    CYGARC_HAL_SAVE_GP();
+
+    // ignore CR
+    if( c == '\r' ) return;
+    
+    HAL_DISABLE_INTERRUPTS(__state);
+    if( c == '\n' )
+    {
+        lcd_dis(LCD_LINE0, &lcd_line[lcd_curline^1][0], base);
+        lcd_dis(LCD_LINE1, &lcd_line[lcd_curline][0], base);
+
+        // Do a line feed
+        lcd_curline ^= 1;
+        lcd_linepos = 0;
+        
+        for( i = 0; i < LCD_LINE_LENGTH; i++ )
+            lcd_line[lcd_curline][i] = ' ';
+
+        HAL_RESTORE_INTERRUPTS(__state);
+        return;
+    }
+
+    // Only allow to be output if there is room on the LCD line
+    if( lcd_linepos < LCD_LINE_LENGTH )
+        lcd_line[lcd_curline][lcd_linepos++] = c;
+
+    HAL_RESTORE_INTERRUPTS(__state);
+}
+
+cyg_uint8
+cyg_hal_plf_lcd_getc(void* __ch_data)
+{
+    return 0;
+}
+
+#if defined(CYGSEM_HAL_VIRTUAL_VECTOR_DIAG) \
+    || defined(CYGPRI_HAL_IMPLEMENTS_IF_SERVICES)
+
+static void
+cyg_hal_plf_lcd_write(void* __ch_data, const cyg_uint8* __buf, 
+                         cyg_uint32 __len)
+{
+    CYGARC_HAL_SAVE_GP();
+
+    while(__len-- > 0)
+        cyg_hal_plf_lcd_putc(__ch_data, *__buf++);
+
+    CYGARC_HAL_RESTORE_GP();
+}
+
+static void
+cyg_hal_plf_lcd_read(void* __ch_data, cyg_uint8* __buf, cyg_uint32 __len)
+{
+    CYGARC_HAL_SAVE_GP();
+
+    while(__len-- > 0)
+        *__buf++ = cyg_hal_plf_lcd_getc(__ch_data);
+
+    CYGARC_HAL_RESTORE_GP();
+}
+
+static int
+cyg_hal_plf_lcd_control(void *__ch_data, __comm_control_cmd_t __func, ...)
+{
+    // Do nothing (yet).
+    return 0;
+}
+
+#endif // CYGSEM_HAL_VIRTUAL_VECTOR_DIAG || CYGPRI_HAL_IMPLEMENTS_IF_SERVICES
+
+static void
+cyg_hal_plf_lcd_init(void)
+{
+    // Init channel
+    init_lcd_channel((cyg_uint8*)LCD_BASE);
+
+#if defined(CYGSEM_HAL_VIRTUAL_VECTOR_DIAG) \
+    || defined(CYGPRI_HAL_IMPLEMENTS_IF_SERVICES)
+    {
+        // Setup procs in the vector table
+        hal_virtual_comm_table_t* comm;
+        int cur = CYGACC_CALL_IF_SET_CONSOLE_COMM()(CYGNUM_CALL_IF_SET_COMM_ID_QUERY_CURRENT);
+
+        // Set channel 2
+        CYGACC_CALL_IF_SET_CONSOLE_COMM()(2);
+        comm = CYGACC_CALL_IF_CONSOLE_PROCS();
+        CYGACC_COMM_IF_CH_DATA_SET(*comm, LCD_BASE);
+        CYGACC_COMM_IF_WRITE_SET(*comm, cyg_hal_plf_lcd_write);
+        CYGACC_COMM_IF_READ_SET(*comm, cyg_hal_plf_lcd_read);
+        CYGACC_COMM_IF_PUTC_SET(*comm, cyg_hal_plf_lcd_putc);
+        CYGACC_COMM_IF_GETC_SET(*comm, cyg_hal_plf_lcd_getc);
+        CYGACC_COMM_IF_CONTROL_SET(*comm, cyg_hal_plf_lcd_control);
+
+        // Restore original console
+        CYGACC_CALL_IF_SET_CONSOLE_COMM()(cur);
+    }
+#endif
+}
+
+//=============================================================================
+// Compatibility with older stubs
+//=============================================================================
+
+#ifndef CYGSEM_HAL_VIRTUAL_VECTOR_DIAG
+
+//-----------------------------------------------------------------------------
 // Assumption: all diagnostic output must be GDB packetized unless
 // this is a configuration for a stand-alone ROM system.
 #if defined(CYG_HAL_STARTUP_ROM) && !defined(CYGSEM_HAL_ROM_MONITOR)
 # define HAL_DIAG_USES_HARDWARE
 #endif
 
-
-//-----------------------------------------------------------------------------
-// Serial diag functions.
-#ifdef CYG_KERNEL_DIAG_SERIAL
-
-// Include the serial driver.
-#define CYG_CMA_PORT CYGHWR_HAL_POWERPC_COGENT_DIAG_PORT
-#include <cma_ser.inl>
+#if (CYGNUM_HAL_VIRTUAL_VECTOR_CONSOLE_CHANNEL == 0)
+# define __BASE ((cyg_uint8*)CYG_DEV_SERIAL_BASE_A)
+#elif (CYGNUM_HAL_VIRTUAL_VECTOR_CONSOLE_CHANNEL == 1)
+# define __BASE ((cyg_uint8*)CYG_DEV_SERIAL_BASE_B)
+#else
+# error "Cannot use LCD"
+#endif
 
 #ifdef HAL_DIAG_USES_HARDWARE
 
 void hal_diag_init(void)
 {
-    hal_cma_init_serial();
+    cyg_hal_plf_comms_init();
 }
 
 void hal_diag_write_char(char __c)
 {
-    hal_cma_put_char(__c);
+    cyg_hal_plf_serial_putc(__BASE, __c);
 }
 
 void hal_diag_read_char(char *c)
 {
-    *c = (char) hal_cma_get_char();
+    *c = cyg_hal_plf_serial_getc(__BASE);
 }
 
 #else  // ifdef HAL_DIAG_USES_HARDWARE
 
 // Initialize diag port
-void hal_diag_init(void)
+void
+hal_diag_init(void)
 {
-#ifdef CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS
-    // assume GDB channel is already set up.
-    if (0) hal_cma_init_serial();       // avoids compiler warning
-#else
-    hal_cma_init_serial();
-#endif
-      
+    // Init devices
+    cyg_hal_plf_comms_init();
 }
 
 void 
@@ -128,14 +603,14 @@ hal_diag_write_char_serial( char c )
 {
     unsigned long __state;
     HAL_DISABLE_INTERRUPTS(__state);
-    hal_cma_put_char(c);
+    cyg_hal_plf_serial_putc(__BASE, c);
     HAL_RESTORE_INTERRUPTS(__state);
 }
 
 void
 hal_diag_read_char(char *c)
 {
-    *c = (char) hal_cma_get_char();
+    *c = cyg_hal_plf_serial_getc(__BASE);
 }
 
 void 
@@ -219,266 +694,11 @@ hal_diag_write_char(char c)
     }
 }
 
-#endif // ifdef HAL_DIAG_USES_HARDWARE
-
-#endif // ifdef CYG_KERNEL_DIAG_SERIAL
-
-//-----------------------------------------------------------------------------
-// Cogent board specific LCD code
-
-#if defined(CYG_KERNEL_DIAG_LCD)
-
-// FEMA 162B 16 character x 2 line LCD
-// base addresses and register offsets *
-
-#define MBD_BASE 0
-
-#define LCD_BASE                (MBD_BASE + 0xEB00007)
-
-#define LCD_DATA                (LCD_BASE + 0x00) // read/write lcd data
-#define LCD_STAT                (LCD_BASE + 0x08) // read lcd busy status
-#define LCD_CMD                 (LCD_BASE + 0x08) // write lcd command
-
-// status register bit definitions
-#define LCD_STAT_BUSY   0x80    // 1 = display busy
-#define LCD_STAT_ADD    0x7F    // bits 0-6 return current display address
-
-// command register definitions
-#define LCD_CMD_RST             0x01    // clear entire display and reset display address
-#define LCD_CMD_HOME            0x02    // reset display address and reset any shifting
-#define LCD_CMD_ECL             0x04    // move cursor left one position on next data write
-#define LCD_CMD_ESL             0x05    // shift display left one position on next data write
-#define LCD_CMD_ECR             0x06    // move cursor right one position on next data write
-#define LCD_CMD_ESR             0x07    // shift display right one position on next data write
-#define LCD_CMD_DOFF            0x08    // display off, cursor off, blinking off
-#define LCD_CMD_BL              0x09    // blink character at current cursor position
-#define LCD_CMD_CUR             0x0A    // enable cursor on
-#define LCD_CMD_DON             0x0C    // turn display on
-#define LCD_CMD_CL              0x10    // move cursor left one position
-#define LCD_CMD_SL              0x14    // shift display left one position
-#define LCD_CMD_CR              0x18    // move cursor right one position
-#define LCD_CMD_SR              0x1C    // shift display right one position
-#define LCD_CMD_MODE            0x38    // sets 8 bits, 2 lines, 5x7 characters
-#define LCD_CMD_ACG             0x40    // bits 0-5 sets the character generator address
-#define LCD_CMD_ADD             0x80    // bits 0-6 sets the display data address to line 1 +
-
-// LCD status values
-#define LCD_OK                  0x00
-#define LCD_ERR                 0x01
-
-#define LCD_LINE0       0x00
-#define LCD_LINE1       0x40
-
-#define LCD_LINE_LENGTH 16
-
-static char lcd_line0[LCD_LINE_LENGTH+1];
-static char lcd_line1[LCD_LINE_LENGTH+1];
-static char *lcd_line[2] = { lcd_line0, lcd_line1 };
-static int lcd_curline = 0;
-static int lcd_linepos = 0;
-
-
-static void lcd_dis(int add, char *string);
-
-externC void diag_write_string (const char*);
-
-void hal_diag_init()
-{
-    cyg_uint8 stat;
-    int i;
-    
-    // wait for not busy
-    // Note: It seems that the LCD isn't quite ready to process commands
-    // when it clears the BUSY flag. Reading the status address an extra
-    // time seems to give it enough breathing room.
-    do { HAL_READ_UINT8 (LCD_STAT, stat); } while (stat & LCD_STAT_BUSY);
-    HAL_READ_UINT8 (LCD_STAT, stat);
-
-    // configure the lcd for 8 bits/char, 2 lines
-    // and 5x7 dot matrix
-    HAL_WRITE_UINT8 (LCD_CMD, LCD_CMD_MODE);
-
-    // wait for not busy
-    do { HAL_READ_UINT8 (LCD_STAT, stat); } while (stat & LCD_STAT_BUSY);
-    HAL_READ_UINT8 (LCD_STAT, stat);
-
-    // turn the LCD display on
-    HAL_WRITE_UINT8 (LCD_CMD, LCD_CMD_DON);
-
-    lcd_curline = 0;
-    lcd_linepos = 0;
-
-    for( i = 0; i < LCD_LINE_LENGTH; i++ )
-        lcd_line[0][i] = lcd_line[1][i] = ' ';
-
-    lcd_line[0][LCD_LINE_LENGTH] = lcd_line[1][LCD_LINE_LENGTH] = 0;
-
-    lcd_dis( LCD_LINE0, lcd_line[0] );
-    lcd_dis( LCD_LINE1, lcd_line[1] );
-
-
-#ifdef CYGSEM_HAL_ROM_MONITOR
-    // It's handy to have the LCD initialized at reset when using it
-    // for debugging output.
-    {
-        diag_write_string ("eCos ROM   " __TIME__ "\n");
-        diag_write_string (__DATE__ "\n");
-    }
-#endif
-}
-
-// this routine writes the string to the LCD
-// display after setting the address to add
-static void lcd_dis(int add, char *string)
-{
-    cyg_uint8 stat;
-    int i;
-
-    // wait for not busy (see Note in hal_diag_init above)
-    do { HAL_READ_UINT8 (LCD_STAT, stat); } while (stat & LCD_STAT_BUSY);
-    HAL_READ_UINT8 (LCD_STAT, stat);
-
-    // write the address
-    HAL_WRITE_UINT8 (LCD_CMD, (LCD_CMD_ADD + add));
-
-    // write the string out to the display stopping when we reach 0
-    for (i = 0; *string != '\0'; i++)
-    {
-        // wait for not busy
-        do { HAL_READ_UINT8 (LCD_STAT, stat); } while (stat & LCD_STAT_BUSY);
-        HAL_READ_UINT8 (LCD_STAT, stat);
+#endif  // ifdef HAL_DIAG_USES_HARDWARE
 
-        // write the data
-        HAL_WRITE_UINT8 (LCD_DATA, *string++);
-    }
-}
-
-void hal_diag_write_char(char c)
-{
-    unsigned long __state;
-    int i;
-    
-    // ignore CR
-    if( c == '\r' ) return;
-    
-    HAL_DISABLE_INTERRUPTS(__state);
-    if( c == '\n' )
-    {
-        lcd_dis( LCD_LINE0, &lcd_line[lcd_curline^1][0] );
-        lcd_dis( LCD_LINE1, &lcd_line[lcd_curline][0] );            
-
-        // Do a line feed
-        lcd_curline ^= 1;
-        lcd_linepos = 0;
-        
-        for( i = 0; i < LCD_LINE_LENGTH; i++ )
-            lcd_line[lcd_curline][i] = ' ';
-
-        HAL_RESTORE_INTERRUPTS(__state);
-        return;
-    }
-
-    // Only allow to be output if there is room on the LCD line
-    if( lcd_linepos < LCD_LINE_LENGTH )
-        lcd_line[lcd_curline][lcd_linepos++] = c;
-
-    HAL_RESTORE_INTERRUPTS(__state);
-}
-
-void hal_diag_read_char(char* c) {}
-
-#endif
-
-
-//---------------------------------------------------------------------------*/
-// PromICE AI interface
-
-
-#if defined(CYG_KERNEL_DIAG_ROMART)
-
-#ifdef CYGPKG_HAL_POWERPC_COGENT
-#define PROMICE_AILOC           0xfff00020
-#endif
-
-// Add this to the LoadICE config file: ailoc 20 19200
-// Note: I couldn't get this to work. jskov
-
-#define PROMICE_BUS_SIZE        16
-#define PROMICE_BURST_SIZE      1
-
-#if PROMICE_BUS_SIZE == 16
+#undef __BASE
 
-typedef volatile struct
-{
-    volatile cyg_uint16 zero;
-//    cyg_uint16          pad1[PROMICE_BURST_SIZE];
-    volatile cyg_uint16 one;
-//    cyg_uint16          pad2[PROMICE_BURST_SIZE];
-    volatile cyg_uint16 data;
-//    cyg_uint16          pad3[PROMICE_BURST_SIZE];
-    volatile cyg_uint16 status;
-
-} AISTRUCT;
-
-#endif
-
-AISTRUCT *AI = (AISTRUCT *)PROMICE_AILOC;
-
-#define PROMICE_STATUS_TDA      0x01
-#define PROMICE_STATUS_HDA      0x02
-#define PROMICE_STATUS_OVR      0x04
-
-void hal_diag_init()
-{
-    volatile cyg_uint8 junk;
- 
-    while( AI->status == 0xCC )
-        continue;
-
-    junk = AI->data;
-}
-
-static void ai_write_char(cyg_uint8 data)
-{
-    volatile cyg_uint8 junk;
-    int i;
-    unsigned long __state;
-    
-    HAL_DISABLE_INTERRUPTS(__state);
-
-    // Wait for tda == 0
-    while( (AI->status & PROMICE_STATUS_TDA) == PROMICE_STATUS_TDA )
-        continue;
-
-    // Send start bit
-    junk = AI->one;
-
-    for( i = 0; i < 8; i++ )
-    {
-        // send ls bit of data
-        if( (data & 1) == 1 )
-            junk = AI->one;
-        else
-            junk = AI->zero;
-
-        // shift down for next bit
-        data >>= 1;
-    }
-
-    // Send stop bit
-    junk = AI->one;
-
-    HAL_RESTORE_INTERRUPTS(__state);
-
-    // all done
-}
-
-void hal_diag_write_char(char c)
-{
-    ai_write_char((cyg_uint8)c);
-}
-
-#endif
+#endif // CYGSEM_HAL_VIRTUAL_VECTOR_DIAG
 
 //-----------------------------------------------------------------------------
 // End of hal_diag.c