index 20b880c..81ceee1 100644
--- a/JTAG2.cpp
+++ b/JTAG2.cpp
@@ -178,10 +178,10 @@ void JTAG2::set_device_descriptor() {
 // *** Target mode set functions ***
 // *** Sets MCU in program mode, if possibe ***
 void JTAG2::enter_progmode() {
-  const uint8_t system_status = UPDI::CPU_mode<0xEF>();
+  uint8_t system_status = UPDI::CPU_mode<0xEE>();
   switch (system_status) {
     // reset in progress, may be caused by WDT
-    case 0x21:
+    case 0x20:
       /* fall-thru */
     // in normal operating mode, reset held (why can reset be held here, anyway? Needs investigation)
     case 0xA2:
@@ -194,7 +194,8 @@ void JTAG2::enter_progmode() {
         break;
       }
       // At this point we need to check if the chip is locked, if so don't attempt to enter program mode
-      if (UPDI::CPU_mode<0x01>()) {
+      if ((system_status = UPDI::CPU_mode<0x01>())) {
+        packet.size_word[0] = 2;
         packet.body[0] = RSP_ILLEGAL_MCU_STATE;
         packet.body[1] = system_status; // return system status (bit 0 will be set to indicate the MCU is locked)
         break;
@@ -243,6 +244,7 @@ void JTAG2::enter_progmode() {
       break;
     default:
       // If we're somehow NOT in programming mode now, that's no good - inform host of this unfortunate state of affairs
+      packet.size_word[0] = 2;
       packet.body[0] = RSP_ILLEGAL_MCU_STATE;
       packet.body[1] = system_status; // return whatever system status caused this error
   }
@@ -292,7 +294,20 @@ void JTAG2::go() {
 
   // *** Read/Write/Erase functions ***
   void JTAG2::read_mem() {
-    if (UPDI::CPU_mode() != 0x08){
+   const uint16_t NumBytes = (packet.body[3] << 8) | packet.body[2];
+
+    uint8_t cpumode = UPDI::CPU_mode() ;
+
+    if ((cpumode & 0x01)) // CPU is locked
+    {
+      packet.body[0] = RSP_MEMORY;
+      packet.size_word[0] = NumBytes + 1;
+      for (uint16_t i = 1; i <= NumBytes; i++)
+        packet.body[i] = 0 ;
+      return ;
+    }
+
+    if (cpumode != 0x08){
       // fail if not in program mode
       packet.size_word[0] = 2;
       packet.body[0] = RSP_ILLEGAL_MCU_STATE;
@@ -300,7 +315,6 @@ void JTAG2::go() {
     }
     else {
       // in program mode
-      const uint16_t NumBytes = (packet.body[3] << 8) | packet.body[2];
       // Calculate size of address
       uint8_t addr_size = (nvm_version == 1) ? 2 : 3;
       // Set UPDI pointer to address