diff --git a/src/main/java/fabrik/simulator/pic16f84/DataRegister.java b/src/main/java/fabrik/simulator/pic16f84/DataRegister.java
index 4485594..1ce8f13 100644
--- a/src/main/java/fabrik/simulator/pic16f84/DataRegister.java
+++ b/src/main/java/fabrik/simulator/pic16f84/DataRegister.java
@@ -78,7 +78,7 @@ public class DataRegister {
     public static void setRegister(int fileAddress, int content){
         int address = determineIndirectAndChange (fileAddress);
         if (Arrays.stream(ioRegisters).anyMatch(i -> i == address)){
-            IOPorts.setRegister(address, content);
+            IOPorts.setRegister(bank() + address, content);
             return;
         }
         if (fileAddress == PCL || fileAddress == 0x80 + PCL){
@@ -104,7 +104,7 @@ public class DataRegister {
     public static void clearBit(int fileAddress, int bit) {
         int address = determineIndirectAndChange (fileAddress);
         if (Arrays.stream(ioRegisters).anyMatch(i -> i == address)){
-            IOPorts.clearBit(address, bit);
+            IOPorts.clearBit(bank() + address, bit);
             return;
         }
         if (!isSyncedRegister(address)) {
@@ -123,7 +123,7 @@ public class DataRegister {
     public static void setBit(int fileAddress, int bit) {
         int address = determineIndirectAndChange (fileAddress);
         if (Arrays.stream(ioRegisters).anyMatch(i -> i == address)){
-            IOPorts.setBit(address, bit);
+            IOPorts.setBit(bank() + address, bit);
             return;
         }
         if (!isSyncedRegister(address)) {
diff --git a/src/main/java/fabrik/simulator/pic16f84/IOPorts.java b/src/main/java/fabrik/simulator/pic16f84/IOPorts.java
index 9ca6f10..1e97eab 100644
--- a/src/main/java/fabrik/simulator/pic16f84/IOPorts.java
+++ b/src/main/java/fabrik/simulator/pic16f84/IOPorts.java
@@ -13,25 +13,23 @@ public class IOPorts {
 
     public static void setBit (int address, int bit){
         if (address < 7) {
-            dataLatch[address - 5] |= (1 << bit);
+            dataLatch[address - PORTA] |= (1 << bit);
         }
         else{
-            trisLatch[address - 0x85] |= (1 << bit);
+            trisLatch[address - TRISA] |= (1 << bit);
         }
         refreshPorts();
     }
 
     public static void clearBit(int address, int bit) {
         if (address < 7) {
-            if (DataRegister.getDirectBit(address, bit) == 1) {
-                int register = DataRegister.getDirectRegister(address);
-                dataLatch[address-A] = register - (int) Math.pow(2, bit);
+            if (((dataLatch[address-PORTA] >> bit)&1) == 1){
+                dataLatch[address-PORTA] -= (int) Math.pow(2, bit);
             }
         }
         else{
             if (DataRegister.getDirectBit(address, bit) == 1) {
-                int register = DataRegister.getDirectRegister(address);
-                trisLatch[address-A] = register - (int) Math.pow(2, bit);
+                trisLatch[address-TRISA] -= (int) Math.pow(2, bit);
             }
         }
         refreshPorts();
@@ -39,16 +37,16 @@ public class IOPorts {
 
     public static void setRegister(int address, int content) {
         if (address < 7) {
-            dataLatch[address - 5] = content;
+            dataLatch[address - PORTA] = content;
         }
         else{
-            trisLatch[address - 0x85] = content;
+            trisLatch[address - TRISA] = content;
         }
         refreshPorts();
     }
 
     private static void refreshPorts() {
-        DataRegister.setDirectRegister(PORTA, (~((~dataLatch[A])&0xFF | trisLatch[A])) & 0x1F);
-        DataRegister.setDirectRegister(PORTB, (~((~dataLatch[B])&0xFF | trisLatch[B])) & 0xFF);
+        System.out.println("PORTA: " + DataRegister.getDirectRegister(PORTA) + "; DATALATCHA: " + dataLatch[A] + "; TRISA: " + trisLatch[A]);
+        System.out.println("PORTB: " + DataRegister.getDirectRegister(PORTB) + "; DATALATCHB: " + dataLatch[B] + "; TRISB: " + trisLatch[B]);
     }
 }
diff --git a/src/main/java/fabrik/simulator/pic16f84/ProgramStack.java b/src/main/java/fabrik/simulator/pic16f84/ProgramStack.java
index 37af106..451f4c2 100644
--- a/src/main/java/fabrik/simulator/pic16f84/ProgramStack.java
+++ b/src/main/java/fabrik/simulator/pic16f84/ProgramStack.java
@@ -17,7 +17,7 @@ public class ProgramStack {
     }
 
     public static void push(int value) {
-        if (returnStack.size() != 8) {
+        if ((returnStack.size() != 8 ) && (returnStack.size() <= stackIndex)){
             returnStack.add(value);
             incIndex();
         }