diff --git a/application/tom_modem/src/main.c b/application/tom_modem/src/main.c
index b1ef125..128222b 100644
--- a/application/tom_modem/src/main.c
+++ b/application/tom_modem/src/main.c
@@ -161,15 +161,18 @@ static void clean_up()
     {
         err_msg("Failed to unlock tty device");
     }
-    if (tcsetattr(s_fds.tty_fd, TCSANOW, &s_fds.old_termios) != 0)
-    {
-        err_msg("Error restoring old tty attributes");
-        return;
-    }
     dbg_msg("Clean up success");
-    tcflush(s_fds.tty_fd, TCIOFLUSH);
     if (s_fds.tty_fd >= 0)
+    {
+        if (tcsetattr(s_fds.tty_fd, TCSANOW, &s_fds.old_termios) != 0)
+        {
+            err_msg("Error restoring old tty attributes");
+            return;
+        }
+        tcflush(s_fds.tty_fd, TCIOFLUSH);
+
         close(s_fds.tty_fd);
+    }
 }
 
 int main(int argc, char *argv[])
@@ -179,6 +182,14 @@ int main(int argc, char *argv[])
     parse_user_input(argc, argv, profile);
     dump_profile();
     #ifdef USE_SEMAPHORE
+    if (profile->op == CLEANUP_SEMAPHORE_OP)
+    {
+        if (unlock_at_port(profile->tty_dev))
+        {
+            err_msg("Failed to unlock tty device");
+        }
+        return SUCCESS;
+    }
     if (profile->tty_dev != NULL)
     {
         if (lock_at_port(profile->tty_dev))
@@ -189,12 +200,15 @@ int main(int argc, char *argv[])
     }
     #endif
     // try open tty devices
+    atexit(clean_up);
+    signal(SIGINT, clean_up);
+    signal(SIGTERM, clean_up);
     if (tty_open_device(profile,fds))
     {
         err_msg("Failed to open tty device");
         return COMM_ERROR;
     }
-    atexit(clean_up);
+
     if (run_op(profile,fds))
     {
         err_msg("Failed to run operation %d", profile->op);
diff --git a/application/tom_modem/src/modem_types.h b/application/tom_modem/src/modem_types.h
index 0deb0dd..ce2c473 100644
--- a/application/tom_modem/src/modem_types.h
+++ b/application/tom_modem/src/modem_types.h
@@ -42,7 +42,10 @@
 #define SMS_SEND_OP_L "sms_send"
 #define SMS_DELETE_OP_S 'd'
 #define SMS_DELETE_OP_L "sms_delete"
-
+#ifdef USE_SEMAPHORE
+#define CLEANUP_SEMAPHORE_OP_S 'C'
+#define CLEANUP_SEMAPHORE_OP_L "cleanup"
+#endif
 #define SET_READ_STORAGE "AT+CPMS=\"%s\""
 #define SET_PDU_FORMAT "AT+CMGF=0"
 #define READ_ALL_SMS "AT+CMGL=4"
@@ -141,7 +144,8 @@ enum OPERATIONS {
     BINARY_AT_OP,
     SMS_READ_OP,
     SMS_SEND_OP,
-    SMS_DELETE_OP
+    SMS_DELETE_OP,
+    CLEANUP_SEMAPHORE_OP
 };
 
 #endif
diff --git a/application/tom_modem/src/utils.c b/application/tom_modem/src/utils.c
index 29e650e..8428e6e 100644
--- a/application/tom_modem/src/utils.c
+++ b/application/tom_modem/src/utils.c
@@ -287,6 +287,8 @@ int match_operation(char *operation_name)
             return SMS_SEND_OP;
         case SMS_DELETE_OP_S:
             return SMS_DELETE_OP;
+        case CLEANUP_SEMAPHORE_OP_S:
+            return CLEANUP_SEMAPHORE_OP;
         default:
             return INVALID_PARAM;
             break;
@@ -314,6 +316,10 @@ int match_operation(char *operation_name)
         {
             return SMS_DELETE_OP;
         }
+        else if (strcmp(operation_name, CLEANUP_SEMAPHORE_OP_L) == 0)
+        {
+            return CLEANUP_SEMAPHORE_OP;
+        }
         else
         {
             return INVALID_PARAM;
@@ -388,11 +394,17 @@ int usage(char* name)
     err_msg("  -D, --debug Debug mode Default: off");
     err_msg("  -p, --sms_pdu <sms pdu>  SMS PDU");
     err_msg("  -i, --sms_index <sms index>  SMS index");
+    #ifdef USE_SEMAPHORE
+    err_msg("  -C, --cleanup Semaphore cleanup");
+    #endif
     err_msg("Example:");
     err_msg("  %s -c ATI -d /dev/ttyUSB2 -b 115200 -B 8 -o at #advance at mode set bautrate and data bit", name);
     err_msg("  %s -c ATI -d /dev/ttyUSB2 # normal at mode", name);
     err_msg("  %s -c ATI -d /dev/ttyUSB2 -o binary_at -c 4154490D0A # means sending ATI to ttyUSB2", name);
     err_msg("  %s -d /dev/mhi_DUN -o r # read sms", name);
+    #ifdef USE_SEMAPHORE
+    err_msg("  %s -d /dev/mhi_DUN  -o C # force cleanup semaphore", name);
+    #endif
     exit(-1);
 }