diff --git a/src/driver/nbiot.be b/src/driver/nbiot.be
index 1b7e70fc9c1cc4f9affa1917937f6615c0b4179d..353e5f057f93472e1e92d133a1429cf69294760f 100644
--- a/src/driver/nbiot.be
+++ b/src/driver/nbiot.be
@@ -12,7 +12,6 @@ class NBIoTRequestType
   static var COAP = 4
   static var NTP = 5
   static var PSM_ENABLE = 6
-  static var PSM_DISABLE = 7
 end
 
 class NBIoTRequest
@@ -206,7 +205,7 @@ class NBIoTDebugProcedure : NBIoTProcedure
   var cmd_debug
 
   def init(ser, request)
-    super(self).init(ser)
+    super(self).init(ser, request)
 
     self.debug = true
     self.cmd_debug = NBIoTCommand(request.cmd + '\r\n', request.rsp, request.retries)
@@ -244,13 +243,41 @@ class NBIoTResetProcedure : NBIoTProcedure
   end
 end
 
+class NBIoTEnablePSMProcedure : NBIoTProcedure
+  var cmd_register
+  var cmd_enable_psm
+  var cmd_enable_sleep
+
+  def init(ser, request)
+    super(self).init(ser, request)
+
+    self.cmd_register = NBIoTCommand('AT+CEREG=5\r\n', 'OK', 10)
+    self.cmd_enable_psm = NBIoTCommand('AT+CPSMS=1,,,\"01000111\",\"00000001\"\r\n', 'OK', 10)
+    self.cmd_enable_sleep = NBIoTCommand('AT+QSCLK=1\r\n', 'OK', 10)
+  end
+
+  def execute()
+    if self.cmd_in_process == nil
+      self.cmd_in_process = self.cmd_register
+    elif self.cmd_in_process == self.cmd_register && self.read_rsp_contains_expected_rsp_or_send()
+      self.cmd_in_process = self.cmd_enable_psm
+    elif self.cmd_in_process == self.cmd_enable_psm && self.read_rsp_contains_expected_rsp_or_send()
+      self.cmd_in_process = self.cmd_enable_sleep
+    elif self.cmd_in_process == self.cmd_enable_sleep
+      self.done = self.read_rsp_contains_expected_rsp_or_send()
+    end
+
+    self.aborted = self.retries_exceeded()
+  end
+end
+
 class NBIoTDisablePSMProcedure : NBIoTProcedure
   var cmd_register
   var cmd_disable_psm
   var cmd_disable_sleep
 
-  def init(ser)
-    super(self).init(ser)
+  def init(ser, request)
+    super(self).init(ser, request)
 
     self.cmd_register = NBIoTCommand('AT+CEREG=1\r\n', 'OK')
     self.cmd_disable_psm = NBIoTCommand('AT+CPSMS=0\r\n', 'OK')
@@ -390,7 +417,7 @@ end
 
 class NBIoTDriverState
   static var INIT = 0
-  static var IDLE = 1
+  static var PSM = 1
   static var RESET = 2
   static var DISABLE_PSM = 3
   static var READY = 4
@@ -420,7 +447,23 @@ class NBIoTDriver
   end
 
   def queue_request(request)
-    self.request_queue.push(request)
+    if self.state != NBIoTDriverState.PSM
+      self.request_queue.push(request)
+          
+      return true
+    end 
+
+    return false
+  end
+
+  def disable_psm()
+    if self.state == NBIoTDriverState.PSM && !self.psm_disabled
+      self.psm_disabled = true
+      
+      return true
+    end 
+
+    return false
   end
 
   def start_procedure(procedure)
@@ -454,8 +497,10 @@ class NBIoTDriver
   end
 
   def every_second()
-    if self.state == NBIoTDriverState.IDLE
-      return
+    if self.state == NBIoTDriverState.PSM
+      if self.psm_disabled
+        self.next_state(NBIoTDriverState.INIT)
+      end
     elif self.state == NBIoTDriverState.INIT
       self.set_psm_pin(1)
       self.next_state(NBIoTDriverState.RESET)
@@ -504,7 +549,9 @@ class NBIoTDriver
           procedure = NBIoTMQTTDisconnectProcedure(self.ser, request)
         elif request.request_type == NBIoTRequestType.NTP
           procedure = NBIoTNTPProcedure(self.ser, request)
-        elif
+        elif request.request_type == NBIoTRequestType.PSM_ENABLE
+          procedure = NBIoTEnablePSMProcedure(self.ser, request)
+        else
           tasmota.log(string.format('NBT: Request with type %i not supported, discarding request', request.request_type), 2)
         end
 
@@ -521,8 +568,18 @@ class NBIoTDriver
           tasmota.log(string.format('NBT: Exceeded retries at command %s, discarding request', cmd), 2)
         end
 
+        if self.procedure.request.request_type == NBIoTRequestType.PSM_ENABLE && self.procedure.is_done()
+          self.psm_disabled = false
+        end
+
         self.finish_procedure(self.procedure.is_aborted())
-        self.next_state(NBIoTDriverState.READY)
+
+        if self.psm_disabled
+          self.next_state(NBIoTDriverState.READY)
+        else
+          self.set_psm_pin(0)
+          self.next_state(NBIoTDriverState.PSM)
+        end
       else
         self.procedure.execute()
       end
@@ -552,37 +609,35 @@ nbiot.init = def (m)
     def enable_power_save_mode(callback)
       var request = NBIoTRequest(NBIoTRequestType.PSM_ENABLE, callback)
 
-      self._driver.queue_request(request)
+      return self._driver.queue_request(request)
     end
 
-    def disable_power_save_mode(callback)
-      var request = NBIoTRequest(NBIoTRequestType.PSM_DISABLE, callback)
-
-      self._driver.queue_request(request)
+    def disable_power_save_mode()
+      return self._driver.disable_psm()
     end
 
     def get_timestamp(callback)
       var request = NBIoTRequest(NBIoTRequestType.NTP, callback)
 
-      self._driver.queue_request(request)
+      return self._driver.queue_request(request)
     end
 
     def mqtt_connect(host, port, client, username, password, callback)
       var request = NBIoTMQTTConnectRequest(host, port, client, username, password, callback)
 
-      self._driver.queue_request(request)
+      return self._driver.queue_request(request)
     end
 
     def mqtt_disconnect(callback)
       var request = NBIoTRequest(NBIoTRequestType.MQTT_DISC, callback)
 
-      self._driver.queue_request(request)
+      return self._driver.queue_request(request)
     end
 
     def mqtt_publish(topic, payload, callback)
       var request = NBIoTMQTTPublishRequest(topic, payload, callback)
 
-      self._driver.queue_request(request)
+      return self._driver.queue_request(request)
     end
 
     def coap_get(host, port, path, query, callback)
@@ -591,9 +646,7 @@ nbiot.init = def (m)
       else
         var request = NBIoTCOAPRequest(host, port, path, 'GET', query, nil, callback)
 
-        self._driver.queue_request(request)
-
-        return true
+        return self._driver.queue_request(request)
       end
     end
 
@@ -603,9 +656,7 @@ nbiot.init = def (m)
       else
         var request = NBIoTCOAPRequest(host, port, path, 'POST', [], payload, callback)
 
-        self._driver.queue_request(request)
-
-        return true
+        return self._driver.queue_request(request)
       end
     end
 
@@ -616,7 +667,7 @@ nbiot.init = def (m)
       
       var request = NBIoTDebugRequest(cmd, rsp, retries)
 
-      self._driver.queue_request(request)
+      return self._driver.queue_request(request)
     end
   end