VNC/ARM: Use VNC server and add support to boot into X11

9 files changed, 776 insertions, 468 deletions

diff --git a/src/dev/arm/RealView.py b/src/dev/arm/RealView.py
index cdc06e4ef..ef3f68a88 100644
--- a/src/dev/arm/RealView.py
+++ b/src/dev/arm/RealView.py
@@ -52,6 +52,14 @@ class AmbaDevice(BasicPioDevice):
     abstract = True
     amba_id = Param.UInt32("ID of AMBA device for kernel detection")
 
+class AmbaIntDevice(AmbaDevice):
+    type = 'AmbaIntDevice'
+    abstract = True
+    gic = Param.Gic(Parent.any, "Gic to use for interrupting")
+    int_num = Param.UInt32("Interrupt number that connects to GIC")
+    int_delay = Param.Latency("100ns",
+            "Time between action and interrupt generation by device")
+
 class AmbaDmaDevice(DmaDevice):
     type = 'AmbaDmaDevice'
     abstract = True
@@ -94,16 +102,17 @@ class Sp804(AmbaDevice):
     clock1 = Param.Clock('1MHz', "Clock speed of the input")
     amba_id = 0x00141804
 
-class Pl050(AmbaDevice):
+class Pl050(AmbaIntDevice):
     type = 'Pl050'
-    gic = Param.Gic(Parent.any, "Gic to use for interrupting")
-    int_num = Param.UInt32("Interrupt number that connects to GIC")
-    int_delay = Param.Latency("100ns", "Time between action and interrupt generation by UART")
+    vnc = Param.VncServer(Parent.any, "Vnc server for remote frame buffer display")
+    is_mouse = Param.Bool(False, "Is this interface a mouse, if not a keyboard")
+    int_delay = '1us'
     amba_id = 0x00141050
 
 class Pl111(AmbaDmaDevice):
     type = 'Pl111'
     clock = Param.Clock('24MHz', "Clock speed of the input")
+    vnc   = Param.VncServer(Parent.any, "Vnc server for remote frame buffer display")
     amba_id = 0x00141111
 
 class RealView(Platform):
@@ -121,7 +130,7 @@ class RealViewPBX(RealView):
     timer1 = Sp804(int_num0=37, int_num1=37, pio_addr=0x10012000)
     clcd = Pl111(pio_addr=0x10020000, int_num=55)
     kmi0   = Pl050(pio_addr=0x10006000, int_num=52)
-    kmi1   = Pl050(pio_addr=0x10007000, int_num=53)
+    kmi1   = Pl050(pio_addr=0x10007000, int_num=53, is_mouse=True)
 
     l2x0_fake     = IsaFake(pio_addr=0x1f002000, pio_size=0xfff)
     flash_fake    = IsaFake(pio_addr=0x40000000, pio_size=0x4000000)
@@ -140,7 +149,7 @@ class RealViewPBX(RealView):
     aaci_fake     = AmbaFake(pio_addr=0x10004000)
     mmc_fake      = AmbaFake(pio_addr=0x10005000)
     rtc_fake      = AmbaFake(pio_addr=0x10017000, amba_id=0x41031)
-
+    cf0_fake      = IsaFake(pio_addr=0x18000000, pio_size=0xfff)
 
 
     # Attach I/O devices that are on chip
@@ -175,6 +184,7 @@ class RealViewPBX(RealView):
        self.mmc_fake.pio      = bus.port
        self.rtc_fake.pio      = bus.port
        self.flash_fake.pio    = bus.port
+       self.cf0_fake.pio      = bus.port
 
 # Reference for memory map and interrupt number
 # RealView Emulation Baseboard User Guide (ARM DUI 0143B)
@@ -187,7 +197,7 @@ class RealViewEB(RealView):
     timer1 = Sp804(int_num0=37, int_num1=37, pio_addr=0x10012000)
     clcd   = Pl111(pio_addr=0x10020000, int_num=23)
     kmi0   = Pl050(pio_addr=0x10006000, int_num=20)
-    kmi1   = Pl050(pio_addr=0x10007000, int_num=21)
+    kmi1   = Pl050(pio_addr=0x10007000, int_num=21, is_mouse=True)
 
     l2x0_fake     = IsaFake(pio_addr=0x1f002000, pio_size=0xfff, warn_access="1")
     dmac_fake     = AmbaFake(pio_addr=0x10030000)
diff --git a/src/dev/arm/amba_device.cc b/src/dev/arm/amba_device.cc
index e5d53d6a3..37eb77ae1 100644
--- a/src/dev/arm/amba_device.cc
+++ b/src/dev/arm/amba_device.cc
@@ -47,11 +47,19 @@
 #include "mem/packet_access.hh"
 
 const uint64_t AmbaVendor = ULL(0xb105f00d00000000);
+
 AmbaDevice::AmbaDevice(const Params *p)
     : BasicPioDevice(p), ambaId(AmbaVendor | p->amba_id)
 {
 }
 
+AmbaIntDevice::AmbaIntDevice(const Params *p)
+    : AmbaDevice(p), intNum(p->int_num), gic(p->gic), intDelay(p->int_delay)
+{
+}
+
+
+
 AmbaDmaDevice::AmbaDmaDevice(const Params *p)
     : DmaDevice(p), ambaId(AmbaVendor | p->amba_id),
       pioAddr(p->pio_addr), pioSize(0),
diff --git a/src/dev/arm/amba_device.hh b/src/dev/arm/amba_device.hh
index 1782fb003..297a78f82 100644
--- a/src/dev/arm/amba_device.hh
+++ b/src/dev/arm/amba_device.hh
@@ -55,6 +55,7 @@
 #include "mem/packet.hh"
 #include "mem/packet_access.hh"
 #include "params/AmbaDevice.hh"
+#include "params/AmbaIntDevice.hh"
 #include "params/AmbaDmaDevice.hh"
 
 namespace AmbaDev {
@@ -81,6 +82,18 @@ class AmbaDevice : public BasicPioDevice
     AmbaDevice(const Params *p);
 };
 
+class AmbaIntDevice : public AmbaDevice
+{
+  protected:
+    int intNum;
+    Gic *gic;
+    Tick intDelay;
+
+  public:
+    typedef AmbaIntDeviceParams Params;
+    AmbaIntDevice(const Params *p);
+};
+
 class AmbaDmaDevice : public DmaDevice
 {
   protected:
diff --git a/src/dev/arm/kmi.cc b/src/dev/arm/kmi.cc
index 6cd61fd09..adf1439b3 100644
--- a/src/dev/arm/kmi.cc
+++ b/src/dev/arm/kmi.cc
@@ -37,21 +37,31 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * Authors: William Wang
+ * Authors: Ali Saidi
+ *          William Wang
  */
 
 #include "base/trace.hh"
+#include "base/vnc/vncserver.hh"
 #include "dev/arm/amba_device.hh"
 #include "dev/arm/kmi.hh"
+#include "dev/ps2.hh"
 #include "mem/packet.hh"
 #include "mem/packet_access.hh"
 
 Pl050::Pl050(const Params *p)
-    : AmbaDevice(p), control(0x00), status(0x43), kmidata(0x00), clkdiv(0x00),
-      intreg(0x00), intNum(p->int_num), gic(p->gic), intDelay(p->int_delay),
-      intEvent(this)
+    : AmbaIntDevice(p), control(0), status(0x43), clkdiv(0), interrupts(0),
+      rawInterrupts(0), ackNext(false), shiftDown(false), vnc(p->vnc),
+      driverInitialized(false), intEvent(this)
 {
     pioSize = 0xfff;
+
+    if (vnc) {
+        if (!p->is_mouse)
+            vnc->setKeyboard(this);
+        else
+            vnc->setMouse(this);
+    }
 }
 
 Tick
@@ -62,28 +72,39 @@ Pl050::read(PacketPtr pkt)
     Addr daddr = pkt->getAddr() - pioAddr;
     pkt->allocate();
 
-    DPRINTF(Pl050, " read register %#x size=%d\n", daddr, pkt->getSize());
 
-    // use a temporary data since the KMI registers are read/written with
-    // different size operations
-    //
     uint32_t data = 0;
 
     switch (daddr) {
       case kmiCr:
+        DPRINTF(Pl050, "Read Commmand: %#x\n", (uint32_t)control);
         data = control;
         break;
       case kmiStat:
+        if (rxQueue.empty())
+            status.rxfull = 0;
+        else
+            status.rxfull = 1;
+
+        DPRINTF(Pl050, "Read Status: %#x\n", (uint32_t)status);
         data = status;
         break;
       case kmiData:
-        data = kmidata;
+        if (rxQueue.empty()) {
+            data = 0;
+        } else {
+            data = rxQueue.front();
+            rxQueue.pop_front();
+        }
+        DPRINTF(Pl050, "Read Data: %#x\n", (uint32_t)data);
+        updateIntStatus();
         break;
       case kmiClkDiv:
         data = clkdiv;
         break;
       case kmiISR:
-        data = intreg;
+        data = interrupts;
+        DPRINTF(Pl050, "Read Interrupts: %#x\n", (uint32_t)interrupts);
         break;
       default:
         if (AmbaDev::readId(pkt, ambaId, pioAddr)) {
@@ -123,65 +144,224 @@ Pl050::write(PacketPtr pkt)
 
     Addr daddr = pkt->getAddr() - pioAddr;
 
-    DPRINTF(Pl050, " write register %#x value %#x size=%d\n", daddr,
-            pkt->get<uint8_t>(), pkt->getSize());
+    assert(pkt->getSize() == sizeof(uint8_t));
 
-    // use a temporary data since the KMI registers are read/written with
-    // different size operations
-    //
-    uint32_t data = 0;
 
-    switch (pkt->getSize()) {
-      case 1:
-        data = pkt->get<uint8_t>();
+    switch (daddr) {
+      case kmiCr:
+        DPRINTF(Pl050, "Write Commmand: %#x\n", (uint32_t)pkt->get<uint8_t>());
+        control = pkt->get<uint8_t>();
+        updateIntStatus();
         break;
-      case 2:
-        data = pkt->get<uint16_t>();
+      case kmiData:
+        DPRINTF(Pl050, "Write Data: %#x\n", (uint32_t)pkt->get<uint8_t>());
+        processCommand(pkt->get<uint8_t>());
+        updateIntStatus();
         break;
-      case 4:
-        data = pkt->get<uint32_t>();
+      case kmiClkDiv:
+        clkdiv = pkt->get<uint8_t>();
         break;
       default:
-        panic("KMI write size too big?\n");
+        warn("Tried to write PL050 at offset %#x that doesn't exist\n", daddr);
         break;
     }
+    pkt->makeAtomicResponse();
+    return pioDelay;
+}
 
+void
+Pl050::processCommand(uint8_t byte)
+{
+    using namespace Ps2;
 
-    switch (daddr) {
-      case kmiCr:
-        control = data;
+    if (ackNext) {
+        ackNext--;
+        rxQueue.push_back(Ack);
+        updateIntStatus();
+        return;
+    }
+
+    switch (byte) {
+      case Ps2Reset:
+        rxQueue.push_back(Ack);
+        rxQueue.push_back(SelfTestPass);
         break;
-      case kmiStat:
-        panic("Tried to write PL050 register(read only) at offset %#x\n",
-              daddr);
+      case SetResolution:
+      case SetRate:
+      case SetStatusLed:
+      case SetScaling1_1:
+      case SetScaling1_2:
+        rxQueue.push_back(Ack);
+        ackNext = 1;
         break;
-      case kmiData:
-        kmidata = data;
+      case ReadId:
+        rxQueue.push_back(Ack);
+        if (params()->is_mouse)
+            rxQueue.push_back(MouseId);
+        else
+            rxQueue.push_back(KeyboardId);
         break;
-      case kmiClkDiv:
-        clkdiv = data;
+      case TpReadId:
+        if (!params()->is_mouse)
+            break;
+        // We're not a trackpoint device, this should make the probe go away
+        rxQueue.push_back(Ack);
+        rxQueue.push_back(0);
+        rxQueue.push_back(0);
+        // fall through
+      case Disable:
+      case Enable:
+        rxQueue.push_back(Ack);
         break;
-      case kmiISR:
-        panic("Tried to write PL050 register(read only) at offset %#x\n",
-              daddr);
+      case StatusRequest:
+        rxQueue.push_back(Ack);
+        rxQueue.push_back(0);
+        rxQueue.push_back(2); // default resolution
+        rxQueue.push_back(100); // default sample rate
         break;
-      default:
-        warn("Tried to write PL050 at offset %#x that doesn't exist\n", daddr);
+      case TouchKitId:
+        ackNext = 2;
+        rxQueue.push_back(Ack);
+        rxQueue.push_back(TouchKitId);
+        rxQueue.push_back(1);
+        rxQueue.push_back('A');
+
+        driverInitialized = true;
         break;
+      default:
+        panic("Unknown byte received: %d\n", byte);
     }
-    pkt->makeAtomicResponse();
-    return pioDelay;
+
+    updateIntStatus();
+}
+
+
+void
+Pl050::updateIntStatus()
+{
+    if (!rxQueue.empty())
+        rawInterrupts.rx = 1;
+    else
+        rawInterrupts.rx = 0;
+
+    interrupts.tx = rawInterrupts.tx & control.txint_enable;
+    interrupts.rx = rawInterrupts.rx & control.rxint_enable;
+
+    DPRINTF(Pl050, "rawInterupts=%#x control=%#x interrupts=%#x\n",
+            (uint32_t)rawInterrupts, (uint32_t)control, (uint32_t)interrupts);
+
+    if (interrupts && !intEvent.scheduled())
+        schedule(intEvent, curTick() + intDelay);
 }
 
 void
 Pl050::generateInterrupt()
 {
-    if (intreg.rxintr || intreg.txintr) {
+
+    if (interrupts) {
         gic->sendInt(intNum);
-        DPRINTF(Pl050, " -- Generated\n");
+        DPRINTF(Pl050, "Generated interrupt\n");
     }
 }
 
+void
+Pl050::mouseAt(uint16_t x, uint16_t y, uint8_t buttons)
+{
+    using namespace Ps2;
+
+    // If the driver hasn't initialized the device yet, no need to try and send
+    // it anything. Similarly we can get vnc mouse events orders of maginture
+    // faster than m5 can process them. Only queue up two sets mouse movements
+    // and don't add more until those are processed.
+    if (!driverInitialized || rxQueue.size() > 10)
+        return;
+
+    // We shouldn't be here unless a vnc server called us in which case
+    // we should have a pointer to it
+    assert(vnc);
+
+    // Convert screen coordinates to touchpad coordinates
+    uint16_t _x = (2047.0/vnc->videoWidth()) * x;
+    uint16_t _y = (2047.0/vnc->videoHeight()) * y;
+
+    rxQueue.push_back(buttons);
+    rxQueue.push_back(_x >> 7);
+    rxQueue.push_back(_x & 0x7f);
+    rxQueue.push_back(_y >> 7);
+    rxQueue.push_back(_y & 0x7f);
+
+    updateIntStatus();
+}
+
+
+void
+Pl050::keyPress(uint32_t key, bool down)
+{
+    using namespace Ps2;
+
+    std::list<uint8_t> keys;
+
+    // convert the X11 keysym into ps2 codes
+    keySymToPs2(key, down, shiftDown, keys);
+
+    // Insert into our queue of charecters
+    rxQueue.splice(rxQueue.end(), keys);
+    updateIntStatus();
+}
+
+void
+Pl050::serialize(std::ostream &os)
+{
+    uint8_t ctrlreg = control;
+    SERIALIZE_SCALAR(ctrlreg);
+
+    uint8_t stsreg = status;
+    SERIALIZE_SCALAR(stsreg);
+    SERIALIZE_SCALAR(clkdiv);
+
+    uint8_t ints = interrupts;
+    SERIALIZE_SCALAR(ints);
+
+    uint8_t raw_ints = rawInterrupts;
+    SERIALIZE_SCALAR(raw_ints);
+
+    SERIALIZE_SCALAR(ackNext);
+    SERIALIZE_SCALAR(shiftDown);
+    SERIALIZE_SCALAR(driverInitialized);
+
+    arrayParamOut(os, "rxQueue", rxQueue);
+}
+
+void
+Pl050::unserialize(Checkpoint *cp, const std::string &section)
+{
+    uint8_t ctrlreg;
+    UNSERIALIZE_SCALAR(ctrlreg);
+    control = ctrlreg;
+
+    uint8_t stsreg;
+    UNSERIALIZE_SCALAR(stsreg);
+    status = stsreg;
+
+    UNSERIALIZE_SCALAR(clkdiv);
+
+    uint8_t ints;
+    UNSERIALIZE_SCALAR(ints);
+    interrupts = ints;
+
+    uint8_t raw_ints;
+    UNSERIALIZE_SCALAR(raw_ints);
+    rawInterrupts = raw_ints;
+
+    UNSERIALIZE_SCALAR(ackNext);
+    UNSERIALIZE_SCALAR(shiftDown);
+    UNSERIALIZE_SCALAR(driverInitialized);
+
+    arrayParamIn(cp, section, "rxQueue", rxQueue);
+}
+
+
+
 Pl050 *
 Pl050Params::create()
 {
diff --git a/src/dev/arm/kmi.hh b/src/dev/arm/kmi.hh
index c96dd55a9..1e25f8974 100644
--- a/src/dev/arm/kmi.hh
+++ b/src/dev/arm/kmi.hh
@@ -48,13 +48,16 @@
 #ifndef __DEV_ARM_PL050_HH__
 #define __DEV_ARM_PL050_HH__
 
+#include <list>
+
 #include "base/range.hh"
-#include "dev/io_device.hh"
+#include "base/vnc/vncserver.hh"
+#include "dev/arm/amba_device.hh"
 #include "params/Pl050.hh"
 
 class Gic;
 
-class Pl050 : public AmbaDevice
+class Pl050 : public AmbaIntDevice, public VncKeyboard, public VncMouse
 {
   protected:
     static const int kmiCr       = 0x000;
@@ -63,34 +66,68 @@ class Pl050 : public AmbaDevice
     static const int kmiClkDiv   = 0x00C;
     static const int kmiISR      = 0x010;
 
-    // control register
-    uint8_t control;
+    BitUnion8(ControlReg)
+        Bitfield<0> force_clock_low;
+        Bitfield<1> force_data_low;
+        Bitfield<2> enable;
+        Bitfield<3> txint_enable;
+        Bitfield<4> rxint_enable;
+        Bitfield<5> type;
+    EndBitUnion(ControlReg)
 
-    // status register
-    uint8_t status;
+    /** control register
+     */
+    ControlReg control;
+
+    /** KMI status register */
+    BitUnion8(StatusReg)
+        Bitfield<0> data_in;
+        Bitfield<1> clk_in;
+        Bitfield<2> rxparity;
+        Bitfield<3> rxbusy;
+        Bitfield<4> rxfull;
+        Bitfield<5> txbusy;
+        Bitfield<6> txempty;
+    EndBitUnion(StatusReg)
+
+    StatusReg status;
+
+    /** clock divisor register
+     * This register is just kept around to satisfy reads after driver does
+     * writes. The divsor does nothing, as we're not actually signaling ps2
+     * serial commands to anything.
+     */
+    uint8_t clkdiv;
 
-    // received data (read) or data to be transmitted (write)
-    uint8_t kmidata;
+    BitUnion8(InterruptReg)
+        Bitfield<0> rx;
+        Bitfield<1> tx;
+    EndBitUnion(InterruptReg)
 
-    // clock divisor register
-    uint8_t clkdiv;
+    /** interrupt status register. */
+    InterruptReg interrupts;
+
+    /** raw interrupt register (unmasked) */
+    InterruptReg rawInterrupts;
 
-    BitUnion8(IntReg)
-    Bitfield<0> txintr;
-    Bitfield<1> rxintr;
-    EndBitUnion(IntReg)
+    /** If the controller should ignore the next data byte and acknowledge it.
+     * The driver is attempting to setup some feature we don't care about
+     */
+    int ackNext;
 
-    /** interrupt mask register. */
-    IntReg intreg;
+    /** is the shift key currently down */
+    bool shiftDown;
 
-    /** Interrupt number to generate */
-    int intNum;
+    /** The vnc server we're connected to (if any) */
+    VncServer *vnc;
 
-    /** Gic to use for interrupting */
-    Gic *gic;
+    /** If the linux driver has initialized the device yet and thus can we send
+     * mouse data */
+    bool driverInitialized;
 
-    /** Delay before interrupting */
-    Tick intDelay;
+    /** Update the status of the interrupt registers and schedule an interrupt
+     * if required */
+    void updateIntStatus();
 
     /** Function to generate interrupt */
     void generateInterrupt();
@@ -98,6 +135,15 @@ class Pl050 : public AmbaDevice
     /** Wrapper to create an event out of the thing */
     EventWrapper<Pl050, &Pl050::generateInterrupt> intEvent;
 
+    /** Receive queue. This list contains all the pending commands that
+     * need to be sent to the driver
+     */
+    std::list<uint8_t> rxQueue;
+
+    /** Handle a command sent to the kmi and respond appropriately
+     */
+    void processCommand(uint8_t byte);
+
   public:
     typedef Pl050Params Params;
     const Params *
@@ -111,12 +157,11 @@ class Pl050 : public AmbaDevice
     virtual Tick read(PacketPtr pkt);
     virtual Tick write(PacketPtr pkt);
 
-    /**
-     * Return if we have an interrupt pending
-     * @return interrupt status
-     * @todo fix me when implementation improves
-     */
-    virtual bool intStatus() { return false; }
+    virtual void mouseAt(uint16_t x, uint16_t y, uint8_t buttons);
+    virtual void keyPress(uint32_t key, bool down);
+
+    virtual void serialize(std::ostream &os);
+    virtual void unserialize(Checkpoint *cp, const std::string &section);
 };
 
-#endif
+#endif // __DEV_ARM_PL050_HH__
diff --git a/src/dev/arm/pl111.cc b/src/dev/arm/pl111.cc
index e597bf272..e884d9b58 100644
--- a/src/dev/arm/pl111.cc
+++ b/src/dev/arm/pl111.cc
@@ -35,9 +35,13 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  * Authors: William Wang
+ *          Ali Saidi
  */
 
+#include "base/bitmap.hh"
+#include "base/output.hh"
 #include "base/trace.hh"
+#include "base/vnc/vncserver.hh"
 #include "dev/arm/amba_device.hh"
 #include "dev/arm/gic.hh"
 #include "dev/arm/pl111.hh"
@@ -50,20 +54,27 @@ using namespace AmbaDev;
 Pl111::Pl111(const Params *p)
     : AmbaDmaDevice(p), lcdTiming0(0), lcdTiming1(0), lcdTiming2(0),
       lcdTiming3(0), lcdUpbase(0), lcdLpbase(0), lcdControl(0), lcdImsc(0),
-      lcdRis(0), lcdMis(0), lcdIcr(0), lcdUpcurr(0), lcdLpcurr(0),
+      lcdRis(0), lcdMis(0),
       clcdCrsrCtrl(0), clcdCrsrConfig(0), clcdCrsrPalette0(0),
       clcdCrsrPalette1(0), clcdCrsrXY(0), clcdCrsrClip(0), clcdCrsrImsc(0),
       clcdCrsrIcr(0), clcdCrsrRis(0), clcdCrsrMis(0), clock(p->clock),
-      height(0), width(0), startTime(0), startAddr(0), maxAddr(0), curAddr(0),
+      vncserver(p->vnc), bmp(NULL), width(LcdMaxWidth), height(LcdMaxHeight),
+      bytesPerPixel(4), startTime(0), startAddr(0), maxAddr(0), curAddr(0),
       waterMark(0), dmaPendingNum(0), readEvent(this), fillFifoEvent(this),
       dmaDoneEvent(maxOutstandingDma, this), intEvent(this)
 {
     pioSize = 0xFFFF;
 
+    pic = simout.create("framebuffer.bmp", true);
+
+    dmaBuffer = new uint8_t[LcdMaxWidth * LcdMaxHeight * sizeof(uint32_t)];
+
     memset(lcdPalette, 0, sizeof(lcdPalette));
     memset(cursorImage, 0, sizeof(cursorImage));
     memset(dmaBuffer, 0, sizeof(dmaBuffer));
-    memset(frameBuffer, 0, sizeof(frameBuffer));
+
+    if (vncserver)
+        vncserver->setFramebufferAddr(dmaBuffer);
 }
 
 // read registers and frame buffer
@@ -75,111 +86,105 @@ Pl111::read(PacketPtr pkt)
 
     uint32_t data = 0;
 
-    if ((pkt->getAddr()& 0xffff0000) == pioAddr) {
+    assert(pkt->getAddr() >= pioAddr &&
+           pkt->getAddr() < pioAddr + pioSize);
 
-        assert(pkt->getAddr() >= pioAddr &&
-               pkt->getAddr() < pioAddr + pioSize);
+    Addr daddr = pkt->getAddr() - pioAddr;
+    pkt->allocate();
 
-        Addr daddr = pkt->getAddr()&0xFFFF;
-        pkt->allocate();
+    DPRINTF(PL111, " read register %#x size=%d\n", daddr, pkt->getSize());
 
-        DPRINTF(PL111, " read register %#x size=%d\n", daddr, pkt->getSize());
-
-        switch (daddr) {
-          case LcdTiming0:
-            data = lcdTiming0;
-            break;
-          case LcdTiming1:
-            data = lcdTiming1;
-            break;
-          case LcdTiming2:
-            data = lcdTiming2;
-            break;
-          case LcdTiming3:
-            data = lcdTiming3;
-            break;
-          case LcdUpBase:
-            data = lcdUpbase;
-            break;
-          case LcdLpBase:
-            data = lcdLpbase;
-            break;
-          case LcdControl:
-            data = lcdControl;
-            break;
-          case LcdImsc:
-            warn("LCD interrupt set/clear function not supported\n");
-            data = lcdImsc;
-            break;
-          case LcdRis:
-            warn("LCD Raw interrupt status function not supported\n");
-            data = lcdRis;
-            break;
-          case LcdMis:
-            warn("LCD Masked interrupt status function not supported\n");
-            data = lcdMis;
-            break;
-          case LcdIcr:
-            panic("LCD register at offset %#x is Write-Only\n", daddr);
-            break;
-          case LcdUpCurr:
-            data = lcdUpcurr;
-            break;
-          case LcdLpCurr:
-            data = lcdLpcurr;
-            break;
-          case ClcdCrsrCtrl:
-            data = clcdCrsrCtrl;
-            break;
-          case ClcdCrsrConfig:
-            data = clcdCrsrConfig;
-            break;
-          case ClcdCrsrPalette0:
-            data = clcdCrsrPalette0;
-            break;
-          case ClcdCrsrPalette1:
-            data = clcdCrsrPalette1;
-            break;
-          case ClcdCrsrXY:
-            data = clcdCrsrXY;
-            break;
-          case ClcdCrsrClip:
-            data = clcdCrsrClip;
-            break;
-          case ClcdCrsrImsc:
-            data = clcdCrsrImsc;
-            break;
-          case ClcdCrsrIcr:
-            panic("CLCD register at offset %#x is Write-Only\n", daddr);
-            break;
-          case ClcdCrsrRis:
-            data = clcdCrsrRis;
-            break;
-          case ClcdCrsrMis:
-            data = clcdCrsrMis;
-            break;
-          default:
-            if (AmbaDev::readId(pkt, AMBA_ID, pioAddr)) {
-                // Hack for variable size accesses
-                data = pkt->get<uint32_t>();
-                break;
-            } else if (daddr >= CrsrImage && daddr <= 0xBFC) {
-                // CURSOR IMAGE
-                int index;
-                index = (daddr - CrsrImage) >> 2;
-                data= cursorImage[index];
-                break;
-            } else if (daddr >= LcdPalette && daddr <= 0x3FC) {
-                // LCD Palette
-                int index;
-                index = (daddr - LcdPalette) >> 2;
-                data = lcdPalette[index];
-                break;
-            } else {
-                panic("Tried to read CLCD register at offset %#x that \
+    switch (daddr) {
+      case LcdTiming0:
+        data = lcdTiming0;
+        break;
+      case LcdTiming1:
+        data = lcdTiming1;
+        break;
+      case LcdTiming2:
+        data = lcdTiming2;
+        break;
+      case LcdTiming3:
+        data = lcdTiming3;
+        break;
+      case LcdUpBase:
+        data = lcdUpbase;
+        break;
+      case LcdLpBase:
+        data = lcdLpbase;
+        break;
+      case LcdControl:
+        data = lcdControl;
+        break;
+      case LcdImsc:
+        data = lcdImsc;
+        break;
+      case LcdRis:
+        data = lcdRis;
+        break;
+      case LcdMis:
+        data = lcdMis;
+        break;
+      case LcdIcr:
+        panic("LCD register at offset %#x is Write-Only\n", daddr);
+        break;
+      case LcdUpCurr:
+        data = curAddr;
+        break;
+      case LcdLpCurr:
+        data = curAddr;
+        break;
+      case ClcdCrsrCtrl:
+        data = clcdCrsrCtrl;
+        break;
+      case ClcdCrsrConfig:
+        data = clcdCrsrConfig;
+        break;
+      case ClcdCrsrPalette0:
+        data = clcdCrsrPalette0;
+        break;
+      case ClcdCrsrPalette1:
+        data = clcdCrsrPalette1;
+        break;
+      case ClcdCrsrXY:
+        data = clcdCrsrXY;
+        break;
+      case ClcdCrsrClip:
+        data = clcdCrsrClip;
+        break;
+      case ClcdCrsrImsc:
+        data = clcdCrsrImsc;
+        break;
+      case ClcdCrsrIcr:
+        panic("CLCD register at offset %#x is Write-Only\n", daddr);
+        break;
+      case ClcdCrsrRis:
+        data = clcdCrsrRis;
+        break;
+      case ClcdCrsrMis:
+        data = clcdCrsrMis;
+        break;
+      default:
+        if (AmbaDev::readId(pkt, AMBA_ID, pioAddr)) {
+            // Hack for variable size accesses
+            data = pkt->get<uint32_t>();
+            break;
+        } else if (daddr >= CrsrImage && daddr <= 0xBFC) {
+            // CURSOR IMAGE
+            int index;
+            index = (daddr - CrsrImage) >> 2;
+            data= cursorImage[index];
+            break;
+        } else if (daddr >= LcdPalette && daddr <= 0x3FC) {
+            // LCD Palette
+            int index;
+            index = (daddr - LcdPalette) >> 2;
+            data = lcdPalette[index];
+            break;
+        } else {
+            panic("Tried to read CLCD register at offset %#x that \
                        doesn't exist\n", daddr);
-                break;
-            }
+            break;
         }
     }
 
@@ -226,119 +231,133 @@ Pl111::write(PacketPtr pkt)
         break;
     }
 
-    if ((pkt->getAddr()& 0xffff0000) == pioAddr) {
+    assert(pkt->getAddr() >= pioAddr &&
+           pkt->getAddr() < pioAddr + pioSize);
 
-        assert(pkt->getAddr() >= pioAddr &&
-               pkt->getAddr() < pioAddr + pioSize);
+    Addr daddr = pkt->getAddr() - pioAddr;
 
-        Addr daddr = pkt->getAddr() - pioAddr;
+    DPRINTF(PL111, " write register %#x value %#x size=%d\n", daddr,
+            pkt->get<uint8_t>(), pkt->getSize());
 
-        DPRINTF(PL111, " write register %#x value %#x size=%d\n", daddr,
-                pkt->get<uint8_t>(), pkt->getSize());
+    switch (daddr) {
+      case LcdTiming0:
+        lcdTiming0 = data;
+        // width = 16 * (PPL+1)
+        width = (lcdTiming0.ppl + 1) << 4;
+        break;
+      case LcdTiming1:
+        lcdTiming1 = data;
+        // height = LPP + 1
+        height = (lcdTiming1.lpp) + 1;
+        break;
+      case LcdTiming2:
+        lcdTiming2 = data;
+        break;
+      case LcdTiming3:
+        lcdTiming3 = data;
+        break;
+      case LcdUpBase:
+        lcdUpbase = data;
+        DPRINTF(PL111, "####### Upper panel base set to: %#x #######\n", lcdUpbase);
+        break;
+      case LcdLpBase:
+        warn("LCD dual screen mode not supported\n");
+        lcdLpbase = data;
+        DPRINTF(PL111, "###### Lower panel base set to: %#x #######\n", lcdLpbase);
+        break;
+      case LcdControl:
+        int old_lcdpwr;
+        old_lcdpwr = lcdControl.lcdpwr;
+        lcdControl = data;
+
+        DPRINTF(PL111, "LCD power is:%d\n", lcdControl.lcdpwr);
+
+        // LCD power enable
+        if (lcdControl.lcdpwr && !old_lcdpwr) {
+            updateVideoParams();
+            DPRINTF(PL111, " lcd size: height %d width %d\n", height, width);
+            waterMark = lcdControl.watermark ? 8 : 4;
+            startDma();
+        }
+        break;
+      case LcdImsc:
+        lcdImsc = data;
+        if (lcdImsc.vcomp)
+            panic("Interrupting on vcomp not supported\n");
 
-        switch (daddr) {
-          case LcdTiming0:
-            lcdTiming0 = data;
-            // width = 16 * (PPL+1)
-            width = (lcdTiming0.ppl + 1) << 4;
-            break;
-          case LcdTiming1:
-            lcdTiming1 = data;
-            // height = LPP + 1
-            height  = (lcdTiming1.lpp) + 1;
-            break;
-          case LcdTiming2:
-            lcdTiming2 = data;
-            break;
-          case LcdTiming3:
-            lcdTiming3 = data;
-            break;
-          case LcdUpBase:
-            lcdUpbase  = data;
-            break;
-          case LcdLpBase:
-            warn("LCD dual screen mode not supported\n");
-            lcdLpbase  = data;
-            break;
-          case LcdControl:
-            int old_lcdpwr;
-            old_lcdpwr = lcdControl.lcdpwr;
-            lcdControl = data;
-            // LCD power enable
-            if (lcdControl.lcdpwr&&!old_lcdpwr) {
-                DPRINTF(PL111, " lcd size: height %d width %d\n", height, width);
-                waterMark = lcdControl.watermark ? 8 : 4;
-                readFramebuffer();
-            }
-            break;
-          case LcdImsc:
-            warn("LCD interrupt mask set/clear not supported\n");
-            lcdImsc    = data;
-            break;
-          case LcdRis:
-            warn("LCD register at offset %#x is Read-Only\n", daddr);
-            break;
-          case LcdMis:
-            warn("LCD register at offset %#x is Read-Only\n", daddr);
-            break;
-          case LcdIcr:
-            warn("LCD interrupt clear not supported\n");
-            lcdIcr     = data;
-            break;
-          case LcdUpCurr:
-            warn("LCD register at offset %#x is Read-Only\n", daddr);
-            break;
-          case LcdLpCurr:
-            warn("LCD register at offset %#x is Read-Only\n", daddr);
-            break;
-          case ClcdCrsrCtrl:
-            clcdCrsrCtrl = data;
-            break;
-          case ClcdCrsrConfig:
-            clcdCrsrConfig = data;
-            break;
-          case ClcdCrsrPalette0:
-            clcdCrsrPalette0 = data;
-            break;
-          case ClcdCrsrPalette1:
-            clcdCrsrPalette1 = data;
-            break;
-          case ClcdCrsrXY:
-            clcdCrsrXY = data;
-            break;
-          case ClcdCrsrClip:
-            clcdCrsrClip = data;
-            break;
-          case ClcdCrsrImsc:
-            clcdCrsrImsc = data;
-            break;
-          case ClcdCrsrIcr:
-            clcdCrsrIcr = data;
-            break;
-          case ClcdCrsrRis:
-            warn("CLCD register at offset %#x is Read-Only\n", daddr);
-            break;
-          case ClcdCrsrMis:
-            warn("CLCD register at offset %#x is Read-Only\n", daddr);
-            break;
-          default:
-            if (daddr >= CrsrImage && daddr <= 0xBFC) {
-                // CURSOR IMAGE
-                int index;
-                index = (daddr - CrsrImage) >> 2;
-                cursorImage[index] = data;
-                break;
-            } else if (daddr >= LcdPalette && daddr <= 0x3FC) {
-                // LCD Palette
-                int index;
-                index = (daddr - LcdPalette) >> 2;
-                lcdPalette[index] = data;
-                break;
-            } else {
-                panic("Tried to write PL111 register at offset %#x that \
+        lcdMis = lcdImsc & lcdRis;
+
+        if (!lcdMis)
+            gic->clearInt(intNum);
+
+         break;
+      case LcdRis:
+        panic("LCD register at offset %#x is Read-Only\n", daddr);
+        break;
+      case LcdMis:
+        panic("LCD register at offset %#x is Read-Only\n", daddr);
+        break;
+      case LcdIcr:
+        lcdRis = lcdRis & ~data;
+        lcdMis = lcdImsc & lcdRis;
+
+        if (!lcdMis)
+            gic->clearInt(intNum);
+
+        break;
+      case LcdUpCurr:
+        panic("LCD register at offset %#x is Read-Only\n", daddr);
+        break;
+      case LcdLpCurr:
+        panic("LCD register at offset %#x is Read-Only\n", daddr);
+        break;
+      case ClcdCrsrCtrl:
+        clcdCrsrCtrl = data;
+        break;
+      case ClcdCrsrConfig:
+        clcdCrsrConfig = data;
+        break;
+      case ClcdCrsrPalette0:
+        clcdCrsrPalette0 = data;
+        break;
+      case ClcdCrsrPalette1:
+        clcdCrsrPalette1 = data;
+        break;
+      case ClcdCrsrXY:
+        clcdCrsrXY = data;
+        break;
+      case ClcdCrsrClip:
+        clcdCrsrClip = data;
+        break;
+      case ClcdCrsrImsc:
+        clcdCrsrImsc = data;
+        break;
+      case ClcdCrsrIcr:
+        clcdCrsrIcr = data;
+        break;
+      case ClcdCrsrRis:
+        panic("CLCD register at offset %#x is Read-Only\n", daddr);
+        break;
+      case ClcdCrsrMis:
+        panic("CLCD register at offset %#x is Read-Only\n", daddr);
+        break;
+      default:
+        if (daddr >= CrsrImage && daddr <= 0xBFC) {
+            // CURSOR IMAGE
+            int index;
+            index = (daddr - CrsrImage) >> 2;
+            cursorImage[index] = data;
+            break;
+        } else if (daddr >= LcdPalette && daddr <= 0x3FC) {
+            // LCD Palette
+            int index;
+            index = (daddr - LcdPalette) >> 2;
+            lcdPalette[index] = data;
+            break;
+        } else {
+            panic("Tried to write PL111 register at offset %#x that \
                        doesn't exist\n", daddr);
-                break;
-            }
+            break;
         }
     }
 
@@ -347,17 +366,75 @@ Pl111::write(PacketPtr pkt)
 }
 
 void
+Pl111::updateVideoParams()
+{
+        if (lcdControl.lcdbpp == bpp24) {
+            bytesPerPixel = 4;
+        } else if (lcdControl.lcdbpp == bpp16m565) {
+            bytesPerPixel = 2;
+        }
+
+        if (vncserver) {
+            if (lcdControl.lcdbpp == bpp24 && lcdControl.bgr)
+                vncserver->setFrameBufferParams(VideoConvert::bgr8888, width,
+                       height);
+            else if (lcdControl.lcdbpp == bpp24 && !lcdControl.bgr)
+                vncserver->setFrameBufferParams(VideoConvert::rgb8888, width,
+                       height);
+            else if (lcdControl.lcdbpp == bpp16m565 && lcdControl.bgr)
+                vncserver->setFrameBufferParams(VideoConvert::bgr565, width,
+                       height);
+            else if (lcdControl.lcdbpp == bpp16m565 && !lcdControl.bgr)
+                vncserver->setFrameBufferParams(VideoConvert::rgb565, width,
+                       height);
+            else
+                panic("Unimplemented video mode\n");
+        }
+
+        if (bmp)
+            delete bmp;
+
+        if (lcdControl.lcdbpp == bpp24 && lcdControl.bgr)
+            bmp = new Bitmap(VideoConvert::bgr8888, width, height, dmaBuffer);
+        else if (lcdControl.lcdbpp == bpp24 && !lcdControl.bgr)
+            bmp = new Bitmap(VideoConvert::rgb8888, width, height, dmaBuffer);
+        else if (lcdControl.lcdbpp == bpp16m565 && lcdControl.bgr)
+            bmp = new Bitmap(VideoConvert::bgr565, width, height, dmaBuffer);
+        else if (lcdControl.lcdbpp == bpp16m565 && !lcdControl.bgr)
+            bmp = new Bitmap(VideoConvert::rgb565, width, height, dmaBuffer);
+        else
+            panic("Unimplemented video mode\n");
+}
+
+void
+Pl111::startDma()
+{
+    if (dmaPendingNum != 0 || readEvent.scheduled())
+        return;
+    readFramebuffer();
+}
+
+void
 Pl111::readFramebuffer()
 {
     // initialization for dma read from frame buffer to dma buffer
-    uint32_t length  = height*width;
-    if (startAddr != lcdUpbase) {
+    uint32_t length = height * width;
+    if (startAddr != lcdUpbase)
         startAddr = lcdUpbase;
-    }
+
+    // Updating base address, interrupt if we're supposed to
+    lcdRis.baseaddr = 1;
+    if (!intEvent.scheduled())
+        schedule(intEvent, nextCycle());
+
     curAddr = 0;
     startTime = curTick();
-    maxAddr = static_cast<Addr>(length*sizeof(uint32_t));
-    dmaPendingNum =0 ;
+
+    maxAddr = static_cast<Addr>(length * bytesPerPixel);
+
+    DPRINTF(PL111, " lcd frame buffer size of %d bytes \n", maxAddr);
+
+    dmaPendingNum = 0;
 
     fillFifo();
 }
@@ -369,11 +446,16 @@ Pl111::fillFifo()
         // concurrent dma reads need different dma done events
         // due to assertion in scheduling state
         ++dmaPendingNum;
-        DPRINTF(PL111, " ++ DMA pending number %d read addr %#x\n",
-                dmaPendingNum, curAddr);
+
         assert(!dmaDoneEvent[dmaPendingNum-1].scheduled());
-        dmaRead(curAddr + startAddr, dmaSize, &dmaDoneEvent[dmaPendingNum-1],
-                curAddr + dmaBuffer);
+
+        // We use a uncachable request here because the requests from the CPU
+        // will be uncacheable as well. If we have uncacheable and cacheable
+        // requests in the memory system for the same address it won't be
+        // pleased
+        dmaPort->dmaAction(MemCmd::ReadReq, curAddr + startAddr, dmaSize,
+                &dmaDoneEvent[dmaPendingNum-1], curAddr + dmaBuffer, 0,
+                Request::UNCACHEABLE);
         curAddr += dmaSize;
     }
 }
@@ -381,27 +463,34 @@ Pl111::fillFifo()
 void
 Pl111::dmaDone()
 {
-    Tick maxFrameTime = lcdTiming2.cpl*height*clock;
+    Tick maxFrameTime = lcdTiming2.cpl * height * clock;
 
     --dmaPendingNum;
 
-    DPRINTF(PL111, " -- DMA pending number %d\n", dmaPendingNum);
-
     if (maxAddr == curAddr && !dmaPendingNum) {
-        if ((curTick() - startTime) > maxFrameTime)
+        if ((curTick() - startTime) > maxFrameTime) {
             warn("CLCD controller buffer underrun, took %d cycles when should"
                  " have taken %d\n", curTick() - startTime, maxFrameTime);
+            lcdRis.underflow = 1;
+            if (!intEvent.scheduled())
+                schedule(intEvent, nextCycle());
+        }
 
-        // double buffering so the vnc server doesn't see a tear in the screen
-        memcpy(frameBuffer, dmaBuffer, maxAddr);
         assert(!readEvent.scheduled());
+        if (vncserver)
+            vncserver->setDirty();
 
         DPRINTF(PL111, "-- write out frame buffer into bmp\n");
-        writeBMP(frameBuffer);
+
+        assert(bmp);
+        pic->seekp(0);
+        bmp->write(pic);
 
         DPRINTF(PL111, "-- schedule next dma read event at %d tick \n",
                 maxFrameTime + curTick());
-        schedule(readEvent, nextCycle(startTime + maxFrameTime));
+
+        if (lcdControl.lcden)
+            schedule(readEvent, nextCycle(startTime + maxFrameTime));
     }
 
     if (dmaPendingNum > (maxOutstandingDma - waterMark))
@@ -409,9 +498,9 @@ Pl111::dmaDone()
 
     if (!fillFifoEvent.scheduled())
         schedule(fillFifoEvent, nextCycle());
-
 }
 
+
 Tick
 Pl111::nextCycle()
 {
@@ -431,33 +520,6 @@ Pl111::nextCycle(Tick beginTick)
     return nextTick;
 }
 
-// write out the frame buffer into a bitmap file
-void
-Pl111::writeBMP(uint32_t* frameBuffer)
-{
-    fstream pic;
-
-    // write out bmp head
-    std::string filename = "./m5out/frameBuffer.bmp";
-    pic.open(filename.c_str(), ios::out|ios::binary);
-    Bitmap bm(pic, height, width);
-
-    DPRINTF(PL111, "-- write out data into bmp\n");
-
-    // write out frame buffer data
-    for (int i = height -1; i >= 0; --i) {
-        for (int j = 0; j< width; ++j) {
-            uint32_t pixel = frameBuffer[i*width + j];
-            pic.write(reinterpret_cast<char*>(&pixel),
-                      sizeof(uint32_t));
-            DPRINTF(PL111, " write pixel data  %#x at addr %#x\n",
-                    pixel, i*width + j);
-        }
-    }
-
-    pic.close();
-}
-
 void
 Pl111::serialize(std::ostream &os)
 {
@@ -490,9 +552,6 @@ Pl111::serialize(std::ostream &os)
     uint8_t lcdMis_serial = lcdMis;
     SERIALIZE_SCALAR(lcdMis_serial);
 
-    uint8_t lcdIcr_serial = lcdIcr;
-    SERIALIZE_SCALAR(lcdIcr_serial);
-
     SERIALIZE_ARRAY(lcdPalette, LcdPaletteSize);
     SERIALIZE_ARRAY(cursorImage, CrsrImageSize);
 
@@ -518,9 +577,9 @@ Pl111::serialize(std::ostream &os)
     SERIALIZE_SCALAR(clock);
     SERIALIZE_SCALAR(height);
     SERIALIZE_SCALAR(width);
+    SERIALIZE_SCALAR(bytesPerPixel);
 
-    SERIALIZE_ARRAY(dmaBuffer, height*width);
-    SERIALIZE_ARRAY(frameBuffer, height*width);
+    SERIALIZE_ARRAY(dmaBuffer, height * width);
     SERIALIZE_SCALAR(startTime);
     SERIALIZE_SCALAR(startAddr);
     SERIALIZE_SCALAR(maxAddr);
@@ -569,10 +628,6 @@ Pl111::unserialize(Checkpoint *cp, const std::string &section)
     UNSERIALIZE_SCALAR(lcdMis_serial);
     lcdMis = lcdMis_serial;
 
-    uint8_t lcdIcr_serial;
-    UNSERIALIZE_SCALAR(lcdIcr_serial);
-    lcdIcr = lcdIcr_serial;
-
     UNSERIALIZE_ARRAY(lcdPalette, LcdPaletteSize);
     UNSERIALIZE_ARRAY(cursorImage, CrsrImageSize);
 
@@ -602,25 +657,29 @@ Pl111::unserialize(Checkpoint *cp, const std::string &section)
     UNSERIALIZE_SCALAR(clock);
     UNSERIALIZE_SCALAR(height);
     UNSERIALIZE_SCALAR(width);
+    UNSERIALIZE_SCALAR(bytesPerPixel);
 
-    UNSERIALIZE_ARRAY(dmaBuffer, height*width);
-    UNSERIALIZE_ARRAY(frameBuffer, height*width);
+    UNSERIALIZE_ARRAY(dmaBuffer, height * width);
     UNSERIALIZE_SCALAR(startTime);
     UNSERIALIZE_SCALAR(startAddr);
     UNSERIALIZE_SCALAR(maxAddr);
     UNSERIALIZE_SCALAR(curAddr);
     UNSERIALIZE_SCALAR(waterMark);
     UNSERIALIZE_SCALAR(dmaPendingNum);
+
+    updateVideoParams();
+    if (vncserver)
+        vncserver->setDirty();
 }
 
 void
 Pl111::generateInterrupt()
 {
     DPRINTF(PL111, "Generate Interrupt: lcdImsc=0x%x lcdRis=0x%x lcdMis=0x%x\n",
-            lcdImsc, lcdRis, lcdMis);
+            (uint32_t)lcdImsc, (uint32_t)lcdRis, (uint32_t)lcdMis);
     lcdMis = lcdImsc & lcdRis;
 
-    if (lcdMis.ffufie || lcdMis.nbupie || lcdMis.vtcpie || lcdMis.ahmeie) {
+    if (lcdMis.underflow || lcdMis.baseaddr || lcdMis.vcomp || lcdMis.ahbmaster) {
         gic->sendInt(intNum);
         DPRINTF(PL111, " -- Generated\n");
     }
@@ -639,15 +698,4 @@ Pl111Params::create()
     return new Pl111(this);
 }
 
-// bitmap class ctor
-Bitmap::Bitmap(std::fstream& bmp, uint16_t h, uint16_t w)
-{
-    Magic  magic  = {{'B','M'}};
-    Header header = {sizeof(Color)*w*h , 0, 0, 54};
-    Info   info   = {sizeof(Info), w, h, 1, sizeof(Color)*8, 0,
-                     ( sizeof(Color) *(w*h) ), 1, 1, 0, 0};
-
-    bmp.write(reinterpret_cast<char*>(&magic),  sizeof(magic));
-    bmp.write(reinterpret_cast<char*>(&header), sizeof(header));
-    bmp.write(reinterpret_cast<char*>(&info),   sizeof(info));
-}
+
diff --git a/src/dev/arm/pl111.hh b/src/dev/arm/pl111.hh
index 4e75af4e8..f36dc6810 100644
--- a/src/dev/arm/pl111.hh
+++ b/src/dev/arm/pl111.hh
@@ -35,6 +35,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  * Authors: William Wang
+ *          Ali Saidi
  */
 
 
@@ -55,6 +56,8 @@
 using namespace std;
 
 class Gic;
+class VncServer;
+class Bitmap;
 
 class Pl111: public AmbaDmaDevice
 {
@@ -96,58 +99,69 @@ class Pl111: public AmbaDmaDevice
     static const int dmaSize            = 8;    // 64 bits
     static const int maxOutstandingDma  = 16;   // 16 deep FIFO of 64 bits
 
+    enum LcdMode {
+        bpp1 = 0,
+        bpp2,
+        bpp4,
+        bpp8,
+        bpp16,
+        bpp24,
+        bpp16m565,
+        bpp12
+    };
+
     BitUnion8(InterruptReg)
-    Bitfield<1> ffufie;
-    Bitfield<2> nbupie;
-    Bitfield<3> vtcpie;
-    Bitfield<4> ahmeie;
+        Bitfield<1> underflow;
+        Bitfield<2> baseaddr;
+        Bitfield<3> vcomp;
+        Bitfield<4> ahbmaster;
     EndBitUnion(InterruptReg)
 
     BitUnion32(TimingReg0)
-    Bitfield<7,2> ppl;
-    Bitfield<15,8> hsw;
-    Bitfield<23,16> hfp;
-    Bitfield<31,24> hbp;
+        Bitfield<7,2> ppl;
+        Bitfield<15,8> hsw;
+        Bitfield<23,16> hfp;
+        Bitfield<31,24> hbp;
     EndBitUnion(TimingReg0)
 
     BitUnion32(TimingReg1)
-    Bitfield<9,0> lpp;
-    Bitfield<15,10> vsw;
-    Bitfield<23,16> vfp;
-    Bitfield<31,24> vbp;
+        Bitfield<9,0> lpp;
+        Bitfield<15,10> vsw;
+        Bitfield<23,16> vfp;
+        Bitfield<31,24> vbp;
     EndBitUnion(TimingReg1)
 
     BitUnion32(TimingReg2)
-    Bitfield<4,0> pcdlo;
-    Bitfield<5> clksel;
-    Bitfield<10,6> acb;
-    Bitfield<11> avs;
-    Bitfield<12> ihs;
-    Bitfield<13> ipc;
-    Bitfield<14> ioe;
-    Bitfield<25,16> cpl;
-    Bitfield<26> bcd;
-    Bitfield<31,27> pcdhi;
+        Bitfield<4,0> pcdlo;
+        Bitfield<5> clksel;
+        Bitfield<10,6> acb;
+        Bitfield<11> avs;
+        Bitfield<12> ihs;
+        Bitfield<13> ipc;
+        Bitfield<14> ioe;
+        Bitfield<25,16> cpl;
+        Bitfield<26> bcd;
+        Bitfield<31,27> pcdhi;
     EndBitUnion(TimingReg2)
 
     BitUnion32(TimingReg3)
-    Bitfield<6,0> led;
-    Bitfield<16> lee;
+        Bitfield<6,0> led;
+        Bitfield<16> lee;
     EndBitUnion(TimingReg3)
 
     BitUnion32(ControlReg)
-    Bitfield<0> lcden;
-    Bitfield<3,1> lcdbpp;
-    Bitfield<4> lcdbw;
-    Bitfield<5> lcdtft;
-    Bitfield<6> lcdmono8;
-    Bitfield<7> lcddual;
-    Bitfield<8> bgr;
-    Bitfield<9> bebo;
-    Bitfield<10> bepo;
-    Bitfield<11> lcdpwr;
-    Bitfield<13,12> lcdvcomp;
-    Bitfield<16> watermark;
+        Bitfield<0> lcden;
+        Bitfield<3,1> lcdbpp;
+        Bitfield<4> lcdbw;
+        Bitfield<5> lcdtft;
+        Bitfield<6> lcdmono8;
+        Bitfield<7> lcddual;
+        Bitfield<8> bgr;
+        Bitfield<9> bebo;
+        Bitfield<10> bepo;
+        Bitfield<11> lcdpwr;
+        Bitfield<13,12> lcdvcomp;
+        Bitfield<16> watermark;
     EndBitUnion(ControlReg)
 
     /** Horizontal axis panel control register */
@@ -180,15 +194,6 @@ class Pl111: public AmbaDmaDevice
     /** Masked interrupt status register */
     InterruptReg lcdMis;
 
-    /** Interrupt clear register */
-    InterruptReg lcdIcr;
-
-    /** Upper panel current address value register - ro */
-    int lcdUpcurr;
-
-    /** Lower panel current address value register - ro */
-    int lcdLpcurr;
-
     /** 256x16-bit color palette registers
      * 256 palette entries organized as 128 locations of two entries per word */
     int lcdPalette[LcdPaletteSize];
@@ -228,17 +233,26 @@ class Pl111: public AmbaDmaDevice
     /** Clock speed */
     Tick clock;
 
-    /** Frame buffer height - lines per panel */
-    uint16_t height;
+    /** VNC server */
+    VncServer *vncserver;
+
+    /** Helper to write out bitmaps */
+    Bitmap *bmp;
+
+    /** Picture of what the current frame buffer looks like */
+    std::ostream *pic;
 
     /** Frame buffer width - pixels per line */
     uint16_t width;
 
-    /** CLCDC supports up to 1024x768 */
-    uint8_t dmaBuffer[LcdMaxWidth * LcdMaxHeight * sizeof(uint32_t)];
+    /** Frame buffer height - lines per panel */
+    uint16_t height;
 
-    /** Double buffering */
-    uint32_t frameBuffer[LcdMaxWidth * LcdMaxHeight];
+    /** Bytes per pixel */
+    uint8_t bytesPerPixel;
+
+    /** CLCDC supports up to 1024x768 */
+    uint8_t *dmaBuffer;
 
     /** Start time for frame buffer dma read */
     Tick startTime;
@@ -258,12 +272,12 @@ class Pl111: public AmbaDmaDevice
     /** Number of pending dma reads */
     int dmaPendingNum;
 
+    /** Send updated parameters to the vnc server */
+    void updateVideoParams();
+
     /** DMA framebuffer read */
     void readFramebuffer();
 
-    /** Write framebuffer to a bmp file */
-    void writeBMP(uint32_t*);
-
     /** Generate dma framebuffer read event */
     void generateReadEvent();
 
@@ -273,6 +287,9 @@ class Pl111: public AmbaDmaDevice
     /** fillFIFO event */
     void fillFifo();
 
+    /** start the dmas off after power is enabled */
+    void startDma();
+
     /** DMA done event */
     void dmaDone();
 
@@ -289,7 +306,7 @@ class Pl111: public AmbaDmaDevice
     /** DMA done event */
     vector<EventWrapper<Pl111, &Pl111::dmaDone> > dmaDoneEvent;
 
-    /** Wrapper to create an event out of the thing */
+    /** Wrapper to create an event out of the interrupt */
     EventWrapper<Pl111, &Pl111::generateInterrupt> intEvent;
 
   public:
@@ -312,57 +329,6 @@ class Pl111: public AmbaDmaDevice
      * @param range_list range list to populate with ranges
      */
     void addressRanges(AddrRangeList &range_list);
-
-    /**
-     * Return if we have an interrupt pending
-     * @return interrupt status
-     * @todo fix me when implementation improves
-     */
-    virtual bool intStatus() { return false; }
-};
-
-// write frame buffer into a bitmap picture
-class  Bitmap
-{
-  public:
-    Bitmap(std::fstream& bmp, uint16_t h, uint16_t w);
-
-  private:
-    struct Magic
-    {
-        unsigned char magic_number[2];
-    } magic;
-
-    struct Header
-    {
-        uint32_t size;
-        uint16_t reserved1;
-        uint16_t reserved2;
-        uint32_t offset;
-    } header;
-
-    struct Info
-    {
-        uint32_t Size;
-        uint32_t Width;
-        uint32_t Height;
-        uint16_t Planes;
-        uint16_t BitCount;
-        uint32_t Compression;
-        uint32_t SizeImage;
-        uint32_t XPelsPerMeter;
-        uint32_t YPelsPerMeter;
-        uint32_t ClrUsed;
-        uint32_t ClrImportant;
-    } info;
-
-    struct Color
-    {
-        unsigned char b;
-        unsigned char g;
-        unsigned char r;
-        unsigned char a;
-    } color;
 };
 
 #endif
diff --git a/src/dev/arm/rv_ctrl.cc b/src/dev/arm/rv_ctrl.cc
index c0ba4c7aa..b1bbc065b 100644
--- a/src/dev/arm/rv_ctrl.cc
+++ b/src/dev/arm/rv_ctrl.cc
@@ -68,6 +68,27 @@ RealViewCtrl::read(PacketPtr pkt)
       case Flash:
         pkt->set<uint32_t>(0);
         break;
+      case Clcd:
+        pkt->set<uint32_t>(0x00001F00);
+        break;
+      case Osc0:
+        pkt->set<uint32_t>(0x00012C5C);
+        break;
+      case Osc1:
+        pkt->set<uint32_t>(0x00002CC0);
+        break;
+      case Osc2:
+        pkt->set<uint32_t>(0x00002C75);
+        break;
+      case Osc3:
+        pkt->set<uint32_t>(0x00020211);
+        break;
+      case Osc4:
+        pkt->set<uint32_t>(0x00002C75);
+        break;
+      case Lock:
+        pkt->set<uint32_t>(sysLock);
+        break;
       default:
         panic("Tried to read RealView I/O at offset %#x that doesn't exist\n", daddr);
         break;
@@ -85,6 +106,15 @@ RealViewCtrl::write(PacketPtr pkt)
     Addr daddr = pkt->getAddr() - pioAddr;
     switch (daddr) {
       case Flash:
+      case Clcd:
+      case Osc0:
+      case Osc1:
+      case Osc2:
+      case Osc3:
+      case Osc4:
+        break;
+      case Lock:
+        sysLock.lockVal = pkt->get<uint16_t>();
         break;
       default:
         panic("Tried to write RVIO at offset %#x that doesn't exist\n", daddr);
diff --git a/src/dev/arm/rv_ctrl.hh b/src/dev/arm/rv_ctrl.hh
index 00a19d715..ceed5ef2f 100644
--- a/src/dev/arm/rv_ctrl.hh
+++ b/src/dev/arm/rv_ctrl.hh
@@ -40,6 +40,7 @@
 #ifndef __DEV_ARM_RV_HH__
 #define __DEV_ARM_RV_HH__
 
+#include "base/bitunion.hh"
 #include "base/range.hh"
 #include "dev/io_device.hh"
 #include "params/RealViewCtrl.hh"
@@ -86,6 +87,14 @@ class RealViewCtrl : public BasicPioDevice
         TestOsc4   = 0xD0
     };
 
+    // system lock value
+    BitUnion32(SysLockReg)
+        Bitfield<15,0> lockVal;
+        Bitfield<16> locked;
+    EndBitUnion(SysLockReg)
+
+    SysLockReg sysLock;
+
   public:
     typedef RealViewCtrlParams Params;
     const Params *
@@ -120,4 +129,3 @@ class RealViewCtrl : public BasicPioDevice
 
 
 #endif // __DEV_ARM_RV_HH__
-