Added latching and reading

diff --git a/palacios/src/devices/8254.c b/palacios/src/devices/8254.c
index 60ae891..8e46d8e 100644 (file)
--- a/palacios/src/devices/8254.c
+++ b/palacios/src/devices/8254.c
@@ -44,6 +44,12 @@ struct channel {
   ushort_t counter;
   ushort_t reload_value;
 
+  ushort_t latched_value;
+  
+  enum {NOTLATCHED,LATCHED} latch_state;
+
+  enum {LSB,MSB} read_state;
+
   uint_t output_pin : 1;
   uint_t gate_input_pin : 1;
 };
@@ -314,7 +320,28 @@ static int handle_channel_write(struct channel * ch, char val) {
 
 
 static int handle_channel_read(struct channel * ch, char * val) {
-  return -1;
+
+  ushort_t *myval;
+
+  if (ch->latch_state==NOTLATCHED) { 
+    myval = &(ch->counter);
+  } else {
+    myval = &(ch->latched_value);
+  }
+
+  if (ch->read_state==LSB) { 
+    *val = ((char*)myval)[0];  // little endian
+    ch->read_state=MSB;
+  } else {
+    *val = ((char*)myval)[1];
+    ch->read_state=LSB;
+    if (ch->latch_state==LATCHED) { 
+      ch->latch_state=NOTLATCHED;
+    }
+  }
+
+  return 0;
+
 }
 
 
@@ -330,7 +357,10 @@ static int handle_channel_cmd(struct channel * ch, struct pit_cmd_word cmd) {
 
   switch (cmd.access_mode) {
   case LATCH_COUNT:
-    return -1;
+    if (ch->latch_state==NOTLATCHED) { 
+      ch->latched_value=ch->counter;
+      ch->latch_state=LATCHED;
+    }
     break;
   case HIBYTE_ONLY:
     ch->access_state = WAITING_HIBYTE;
@@ -500,6 +530,10 @@ static void init_channel(struct channel * ch) {
   ch->output_pin = 0;
   ch->gate_input_pin = 0;
 
+  ch->latched_value=0;
+  ch->latch_state=NOTLATCHED;
+  ch->read_state=LSB;
+
   return;
 }