diff --git a/lib/modem-commands.js b/lib/modem-commands.js
index aea547b..2a4221a 100644
--- a/lib/modem-commands.js
+++ b/lib/modem-commands.js
@@ -26,7 +26,22 @@ exports.writeToPortAndWaitForReadline = function writeToPortAndWaitForReadline(c
     return new Promise(async (resolve) => {
         const parser = new ParserReadline({ delimiter: parsers.PARSER_READLINE_DELIMITER });
         parser.on('data', (data) => {
-            logger.verbose('INCOMING', { parser: 'customParserReadLine', data: `${data.toString()}${parsers.PARSER_READLINE_DELIMITER}` });
+            port.unpipe(parser);
+            mutex.unlock(lockName || MUTEX_COMMAND, cmd.trim());
+            resolve(data);
+        });
+
+        await mutex.lock(lockName || MUTEX_COMMAND, cmd.trim());
+        port.pipe(parser);
+        await writeToPort(cmd);
+    });
+};
+
+exports.writeToPortAndWaitForOkOrError = function writeToPortAndWaitForOkOrError(cmd, lockName) {
+    return new Promise(async (resolve) => {
+        const parser = new ParserRegex({ regex: parsers.PARSER_WAIT_FOR_OK_OR_ERROR_REGEX });
+        parser.on('data', (data) => {
+            logger.verbose('INCOMING', { parser: 'customParserWaitForOkOrError', data: data.toString() });
             port.unpipe(parser);
             mutex.unlock(lockName || MUTEX_COMMAND, cmd.trim());
             resolve(data);
diff --git a/modem-tester.js b/modem-tester.js
index c1dcdc3..d8115a0 100644
--- a/modem-tester.js
+++ b/modem-tester.js
@@ -49,9 +49,9 @@ port = new SerialPort(config.modem.device, { baudRate: 115200 }, async (err) =>
 
     await writeToPortDelayed('AT\r');
 
-    await modemCommands.writeToPortAndWaitForReadline('AT&FE0\r');
-    await modemCommands.writeToPortAndWaitForReadline('AT+CMGF=0\r');
-    await modemCommands.writeToPortAndWaitForReadline('AT+CNMI=1,2,0,1,0\r');
+    await modemCommands.writeToPortAndWaitForOkOrError('AT&FE0\r');
+    await modemCommands.writeToPortAndWaitForOkOrError('AT+CMGF=0\r');
+    await modemCommands.writeToPortAndWaitForOkOrError('AT+CNMI=1,2,0,1,0\r');
 
     const imei = await modemCommands.queryIMEI();
     logger.info(`**** IMEI: ${imei}`);