Fixed set_gpio

Grzegorz Jabłoński
1 parent dc1560f5
Showing 2 changed files with 31 additions and 8 deletions
hw/at91_pio.c
monitor.c
@@ -88,6 +88,7 @@ typedef struct PIOState {
 void at91_pio_set_pin(void *opaque, int pin, int level)
 {
+    printf("In at91_pio_set_pin, opaque=%p, pin=%d, level=%d\n", opaque, pin, level);
     PIOState *s = opaque;
     int mask = 1 << (pin % PIO_PINS);
     int input_set = pin / PIO_PINS;
@@ -97,6 +98,8 @@ void at91_pio_set_pin(void *opaque, int pin, int level)
     if (input_set == 0) {
         /* PIO pin -> Peripheral / IO */
+        printf("In at91_pio_set_pin, s->osr=%08x, s->psr=%08x, mask=%08x\n",s->osr, s->psr, mask);
+
         /* Skip input if output mode is enabled for the pin */
         if (s->osr & mask)
             return;
@@ -104,9 +107,11 @@ void at91_pio_set_pin(void *opaque, int pin, int level)
         if (s->psr & mask) {
             saved_pdsr = s->pdsr;
             s->pdsr &= ~mask;
+            s->unknown_state &= ~mask;
             if (level == -1) {
                 s->unknown_state |= mask;
             } else if (level) {
+                printf("In at91_pio_set_pin, setting level\n");
                 s->unknown_state &= ~mask;
                 s->pdsr |= mask;
             }
@@ -117,6 +122,8 @@ void at91_pio_set_pin(void *opaque, int pin, int level)
         } else {
             qemu_set_irq(s->out[PIO_PINS + (output_set * PIO_PINS)], level);
         }
+        printf("In at91_pio_set_pin, s->pdsr=%08x\n",s->pdsr);
+
     } else {
         /* Peripheral -> PIO pin */
         if ((~s->psr & mask) && input_set == output_set) {
@@ -141,6 +148,10 @@ static uint32_t at91_pio_mem_read(void *opaque, target_phys_addr_t offset)
     case PIO_ODSR:
         return s->odsr;
     case PIO_PDSR:
+        printf("In at91_pio_mem_read, reading PDSR, s->pdsr=%08x, s->unknown_state=%08x, retval=%08x\n",
+        s->pdsr, s->unknown_state, 
+            (s->pdsr & ~s->unknown_state) |
+            (s->ppusr & s->unknown_state));
         return
             (s->pdsr & ~s->unknown_state) |
             (s->ppusr & s->unknown_state);
@@ -322,12 +333,14 @@ static void at91_pio_reset(void *opaque)
     s->unknown_state = 0xffffffff;
 }
-void* at91_io_state = NULL;
+void* at91_io_state[5];
+int at91_io_cnt = 0;
 static void at91_pio_init(SysBusDevice *dev)
 {
+    printf("at91_pio_init called\n");
     PIOState *s = FROM_SYSBUS(typeof (*s), dev);
-    at91_io_state = s;
+    at91_io_state[at91_io_cnt++] = s;
     int pio_regs;
     sysbus_init_irq(dev, &s->parent_irq);
@@ -521,16 +521,26 @@ static void do_screen_dump(Monitor *mon, const char *filename)
 static void do_set_gpio(Monitor* mon, const char* port, const char* value)
 {
-  extern void* at91_io_state;
+  extern void* at91_io_state[5];
   extern void at91_pio_set_pin(void *opaque, int pin, int level);
-  if (at91_io_state)
+  if (strlen(port)<1)
   {
-     printf("Setting gpio: %s %s\n", port, value);
-     at91_pio_set_pin(at91_io_state, atoi(port), atoi(value));
+       printf("Invalid port\n");
+       return;
+  }
+  int channel = port[0]-'A';
+  if ((channel >= 0) && (channel < 5))
+  {
+    if (at91_io_state[channel])
+    {
+       printf("Setting gpio: %s %s\n", port, value);
+       at91_pio_set_pin(at91_io_state[channel], atoi(port+1), atoi(value));
+    }
+    else
+      printf("Cannot set pin: device pointer is NULL\n");
   }
   else
-     printf("Cannot set pin: device pointer is NULL\n");
-
+   printf("Invalid channel\n");
 }
 static void do_logfile(Monitor *mon, const char *filename)