diff --git a/pypcc/i2cserv/i2cthread.py b/pypcc/i2cserv/i2cthread.py
index 0714c291372c4033832d6e6b63778fa88269b367..c5d6564f5b13b30aa44f698a0c26324c0bfd9c49 100644
--- a/pypcc/i2cserv/i2cthread.py
+++ b/pypcc/i2cserv/i2cthread.py
@@ -29,7 +29,28 @@ class AttrDict(object):
         except KeyError:
             raise AttributeError
 
-def runmethod(conf,Qout,methodid,mask,PQ):
+def get_lock(lock):
+        if lock is None: return True 
+        try:
+                lock.acquire()
+                return True
+        except:
+                logging.error("I2C lock timeout!!")
+                return False
+                #continue;
+#              print('lock');
+
+def release_lock(lock):
+    if lock is None: return True 
+    try:
+        lock.release()
+        return True
+    except:
+        logging.error("I2C lock release error")
+        return False
+
+
+def runmethod(conf,Qout,methodid,mask,PQ,lock=None):
 #    print("Run method",methodid,mask)
     var1=conf.getmethod(methodid)
     drv=var1.get('drivercls');
@@ -39,7 +60,9 @@ def runmethod(conf,Qout,methodid,mask,PQ):
             logging.info(str(("Run instruction",key,value,mask)));
             if mask is None: mask=[]
             if (key=='WAIT'):
+                release_lock(lock)
                 sleep(value[0]/1000.)
+                get_lock(lock)
                 continue;
             v1=conf.getvarid(key)
             if not(v1 is None): 
@@ -147,13 +170,7 @@ def I2Cserver(Qin,Qout,name,lock=None):
 #           print("TODO: Set busy")
 #           self.statevar.set_value("busy");
            #print("SetVar",item)#,self.conf.variables[item.id])
-           if not(lock is None): 
-              try:
-                lock.acquire()
-              except:
-                logging.error("I2C lock timeout!!")
-                #continue;
-#              print('lock');
+           get_lock(lock)
            try:
             if (item.type==InstType.varSet):
                setvar(conf,Qout,item.id,item.data,item.mask)
@@ -162,14 +179,10 @@ def I2Cserver(Qin,Qout,name,lock=None):
                getvar(conf,Qout,item.id,item.mask)
            #    var1=self.conf.variables[item.id]
            #    self.OPCUAReadVariable(item.id,var1,item.data,item.mask)
-            elif (item.type==InstType.method): runmethod(conf,Qout,item.id,item.mask,PQ)
+            elif (item.type==InstType.method): runmethod(conf,Qout,item.id,item.mask,PQ,lock=lock)
             else: print("OPCUA call not implemented!");
            finally:
-             if not(lock is None): 
-                  try:
-                     lock.release()
-                  except:
-                     logging.error("I2C lock release error")
+             release_lock(lock)
 #                  print('unlock');
 #           print("TODO: Set ready")
         logging.info("End i2c process "+name)