diff --git a/src/hsm/cmd_decrypt_asym.c b/src/hsm/cmd_decrypt_asym.c
index 559ed24..f4835dc 100644
--- a/src/hsm/cmd_decrypt_asym.c
+++ b/src/hsm/cmd_decrypt_asym.c
@@ -120,8 +120,8 @@ int cmd_decrypt_asym() {
         size_t olen = 0;
         res_APDU[0] = 0x04;
         r = mbedtls_ecdh_calc_secret(&ctx, &olen, res_APDU+1, MBEDTLS_ECP_MAX_BYTES, random_gen, NULL);
+        mbedtls_ecdh_free(&ctx);
         if (r != 0) {
-            mbedtls_ecdh_free(&ctx);
             return SW_EXEC_ERROR();
         }
         if (p2 == ALGO_EC_DH)
@@ -161,7 +161,6 @@ int cmd_decrypt_asym() {
             }
             return SW_REFERENCE_NOT_FOUND();
         }
-        mbedtls_ecdh_free(&ctx);
     }
     else
         return SW_WRONG_P1P2();
diff --git a/src/hsm/cmd_derive_asym.c b/src/hsm/cmd_derive_asym.c
index 00a8303..34d30b7 100644
--- a/src/hsm/cmd_derive_asym.c
+++ b/src/hsm/cmd_derive_asym.c
@@ -19,7 +19,7 @@
 #include "mbedtls/ecdsa.h"
 #include "crypto_utils.h"
 #include "sc_hsm.h"
-
+#include "cvc.h"
 
 #define MBEDTLS_ERR_ERROR_CORRUPTION_DETECTED -0x006E
 #define MOD_ADD( N )                                                    \
@@ -72,29 +72,24 @@ int cmd_derive_asym() {
             return SW_DATA_INVALID();
         }
         r = mbedtls_mpi_add_mod(&ctx.grp, &nd, &ctx.d, &a);
+        mbedtls_mpi_free(&a);
         if (r != 0) {
             mbedtls_ecdsa_free(&ctx);
-            mbedtls_mpi_free(&a);
             mbedtls_mpi_free(&nd);
             return SW_EXEC_ERROR();
         }
         r = mbedtls_mpi_copy(&ctx.d, &nd);
+        mbedtls_mpi_free(&nd);
         if (r != 0) {
             mbedtls_ecdsa_free(&ctx);
-            mbedtls_mpi_free(&a);
-            mbedtls_mpi_free(&nd);
             return SW_EXEC_ERROR();
         }
         r = store_keys(&ctx, HSM_KEY_EC, dest_id);
         if (r != CCID_OK) {
             mbedtls_ecdsa_free(&ctx);
-            mbedtls_mpi_free(&a);
-            mbedtls_mpi_free(&nd);
             return SW_EXEC_ERROR();
         }
         mbedtls_ecdsa_free(&ctx);
-        mbedtls_mpi_free(&a);
-        mbedtls_mpi_free(&nd);
     }
     else
         return SW_WRONG_DATA();
diff --git a/src/hsm/cmd_extras.c b/src/hsm/cmd_extras.c
index 440ed9b..c215274 100644
--- a/src/hsm/cmd_extras.c
+++ b/src/hsm/cmd_extras.c
@@ -15,14 +15,20 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "common.h"
+#include "mbedtls/ecdh.h"
 #include "sc_hsm.h"
 #include "hardware/rtc.h"
 #include "files.h"
+#include "random.h"
+#include "kek.h"
+#include "mbedtls/hkdf.h"
+#include "mbedtls/chachapoly.h"
 
 int cmd_extras() {
-    if (P2(apdu) != 0x0)
-        return SW_INCORRECT_P1P2();
     if (P1(apdu) == 0xA) { //datetime operations
+        if (P2(apdu) != 0x0)
+            return SW_INCORRECT_P1P2();
         if (apdu.nc == 0) {
             datetime_t dt;
             if (!rtc_get_datetime(&dt))
@@ -52,6 +58,8 @@ int cmd_extras() {
         }
     }
     else if (P1(apdu) == 0x6) { //dynamic options
+        if (P2(apdu) != 0x0)
+            return SW_INCORRECT_P1P2();
         if (apdu.nc > sizeof(uint8_t))
             return SW_WRONG_LENGTH();
         uint16_t opts = get_device_options();
@@ -66,6 +74,86 @@ int cmd_extras() {
             low_flash_available();
         }
     }
+    else if (P1(apdu) == 0x3A) { // secure lock
+        if (apdu.nc == 0) {
+            return SW_WRONG_LENGTH();
+        }
+        if (P2(apdu) == 0x01) { // Key Agreement
+            mbedtls_ecdh_context hkey;
+            mbedtls_ecdh_init(&hkey);
+            mbedtls_ecdh_setup(&hkey, MBEDTLS_ECP_DP_SECP256R1);
+            int ret = mbedtls_ecdh_gen_public(&hkey.ctx.mbed_ecdh.grp, &hkey.ctx.mbed_ecdh.d, &hkey.ctx.mbed_ecdh.Q, random_gen, NULL);
+            mbedtls_mpi_lset(&hkey.ctx.mbed_ecdh.Qp.Z, 1);
+            ret = mbedtls_ecp_point_read_binary(&hkey.ctx.mbed_ecdh.grp, &hkey.ctx.mbed_ecdh.Qp, apdu.data, apdu.nc);
+            if (ret != 0) {
+                mbedtls_ecdh_free(&hkey);
+                return SW_WRONG_DATA();
+            }
+            memcpy(mse.Qpt, apdu.data, sizeof(mse.Qpt));
+
+            uint8_t buf[MBEDTLS_ECP_MAX_BYTES];
+            size_t olen = 0;
+            ret = mbedtls_ecdh_calc_secret(&hkey, &olen, buf, MBEDTLS_ECP_MAX_BYTES, random_gen, NULL);
+            if (ret != 0) {
+                mbedtls_ecdh_free(&hkey);
+                mbedtls_platform_zeroize(buf, sizeof(buf));
+                return SW_WRONG_DATA();
+            }
+            ret = mbedtls_hkdf(mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), NULL, 0, buf, olen, mse.Qpt, sizeof(mse.Qpt), mse.key_enc, sizeof(mse.key_enc));
+            mbedtls_platform_zeroize(buf, sizeof(buf));
+            if (ret != 0) {
+                mbedtls_ecdh_free(&hkey);
+                return SW_EXEC_ERROR();
+            }
+
+            ret = mbedtls_ecp_point_write_binary(&hkey.ctx.mbed_ecdh.grp, &hkey.ctx.mbed_ecdh.Q, MBEDTLS_ECP_PF_UNCOMPRESSED, &olen, res_APDU, 4096);
+            mbedtls_ecdh_free(&hkey);
+            if (ret != 0) {
+                return SW_EXEC_ERROR();
+            }
+            mse.init = true;
+            res_APDU_size = olen;
+        }
+        else if (P2(apdu) == 0x02 || P2(apdu) == 0x03 || P2(apdu) == 0x04) {
+            if (mse.init == false)
+                return SW_COMMAND_NOT_ALLOWED();
+
+            int ret = mse_decrypt_ct(apdu.data, apdu.nc);
+            if (ret != 0) {
+                return SW_WRONG_DATA();
+            }
+            if (P2(apdu) == 0x02 || P2(apdu) == 0x04) { // Enable
+                uint16_t opts = get_device_options();
+                uint8_t newopts[] = { opts >> 8, (opts & 0xff) };
+                if ((P2(apdu) == 0x02 && !(opts & HSM_OPT_SECURE_LOCK)) || (P2(apdu) == 0x04 && (opts & HSM_OPT_SECURE_LOCK))) {
+                    uint16_t tfids[] = { EF_MKEK, EF_MKEK_SO };
+                    for (int t = 0; t < sizeof(tfids)/sizeof(uint16_t); t++) {
+                        file_t *tf = search_by_fid(tfids[t], NULL, SPECIFY_EF);
+                        if (tf) {
+                            uint8_t *tmp = (uint8_t *)calloc(1, file_get_size(tf));
+                            memcpy(tmp, file_get_data(tf), file_get_size(tf));
+                            for (int i = 0; i < MKEK_KEY_SIZE; i++) {
+                                MKEK_KEY(tmp)[i] ^= apdu.data[i];
+                            }
+                            flash_write_data_to_file(tf, tmp, file_get_size(tf));
+                            free(tmp);
+                        }
+                    }
+                }
+                if (P2(apdu) == 0x02)
+                    newopts[0] |= HSM_OPT_SECURE_LOCK >> 8;
+                else if (P2(apdu) == 0x04)
+                    newopts[0] &= ~HSM_OPT_SECURE_LOCK >> 8;
+                file_t *tf = search_by_fid(EF_DEVOPS, NULL, SPECIFY_EF);
+                flash_write_data_to_file(tf, newopts, sizeof(newopts));
+                low_flash_available();
+            }
+            else if (P2(apdu) == 0x03) {
+                memcpy(mkek_mask, apdu.data, apdu.nc);
+                has_mkek_mask = true;
+            }
+        }
+    }
     else
         return SW_INCORRECT_P1P2();
     return SW_OK();
diff --git a/src/hsm/cmd_general_authenticate.c b/src/hsm/cmd_general_authenticate.c
index 77b16b8..6d1620a 100644
--- a/src/hsm/cmd_general_authenticate.c
+++ b/src/hsm/cmd_general_authenticate.c
@@ -80,7 +80,7 @@ int cmd_general_authenticate() {
             sm_derive_all_keys(derived, olen);
 
             uint8_t *t = (uint8_t *)calloc(1, pubkey_len+16);
-            memcpy(t, "\x7F\x49\x3F\x06\x0A", 5);
+            memcpy(t, "\x7F\x49\x4F\x06\x0A", 5);
             if (sm_get_protocol() == MSE_AES)
                 memcpy(t+5, OID_ID_CA_ECDH_AES_CBC_CMAC_128, 10);
             t[15] = 0x86;
diff --git a/src/hsm/cmd_keypair_gen.c b/src/hsm/cmd_keypair_gen.c
index 82f3758..7497f13 100644
--- a/src/hsm/cmd_keypair_gen.c
+++ b/src/hsm/cmd_keypair_gen.c
@@ -122,14 +122,18 @@ int cmd_keypair_gen() {
                     }
                 }
                 if ((res_APDU_size = asn1_cvc_aut(&ecdsa, HSM_KEY_EC, res_APDU, 4096, ext, ext_len)) == 0) {
-                    return SW_EXEC_ERROR();
-                }
-	            ret = store_keys(&ecdsa, HSM_KEY_EC, key_id);
-	            if (ret != CCID_OK) {
+                    if (ext)
+                        free(ext);
                     mbedtls_ecdsa_free(&ecdsa);
                     return SW_EXEC_ERROR();
                 }
+                if (ext)
+                    free(ext);
+                ret = store_keys(&ecdsa, HSM_KEY_EC, key_id);
                 mbedtls_ecdsa_free(&ecdsa);
+	            if (ret != CCID_OK) {
+                    return SW_EXEC_ERROR();
+                }
             }
 
         }
@@ -142,8 +146,8 @@ int cmd_keypair_gen() {
     ret = flash_write_data_to_file(fpk, res_APDU, res_APDU_size);
     if (ret != 0)
         return SW_EXEC_ERROR();
-    if (apdu.ne == 0)
-        apdu.ne = res_APDU_size;
+    //if (apdu.ne == 0)
+    //    apdu.ne = res_APDU_size;
     low_flash_available();
     return SW_OK();
 }
diff --git a/src/hsm/cvc.c b/src/hsm/cvc.c
index a679442..3783a01 100644
--- a/src/hsm/cvc.c
+++ b/src/hsm/cvc.c
@@ -68,7 +68,7 @@ size_t asn1_cvc_public_key_ecdsa(mbedtls_ecdsa_context *ecdsa, uint8_t *buf, siz
     size_t b_size = mbedtls_mpi_size(&ecdsa->grp.B), g_size = 1+mbedtls_mpi_size(&ecdsa->grp.G.X)+mbedtls_mpi_size(&ecdsa->grp.G.X);
     size_t o_size = mbedtls_mpi_size(&ecdsa->grp.N), y_size = 1+mbedtls_mpi_size(&ecdsa->Q.X)+mbedtls_mpi_size(&ecdsa->Q.X);
     size_t c_size = 1;
-    size_t ptot_size = asn1_len_tag(0x81, p_size), atot_size = asn1_len_tag(0x82, a_size ? a_size : (pointA[ecdsa->grp.id] ? p_size : 0));
+    size_t ptot_size = asn1_len_tag(0x81, p_size), atot_size = asn1_len_tag(0x82, a_size ? a_size : (pointA[ecdsa->grp.id] && ecdsa->grp.id < 6 ? p_size : 1));
     size_t btot_size = asn1_len_tag(0x83, b_size), gtot_size = asn1_len_tag(0x84, g_size);
     size_t otot_size = asn1_len_tag(0x85, o_size), ytot_size = asn1_len_tag(0x86, y_size);
     size_t ctot_size = asn1_len_tag(0x87, c_size);
@@ -90,11 +90,12 @@ size_t asn1_cvc_public_key_ecdsa(mbedtls_ecdsa_context *ecdsa, uint8_t *buf, siz
         *p++ = 0x82; p += format_tlv_len(a_size, p); mbedtls_mpi_write_binary(&ecdsa->grp.A, p, a_size); p += a_size;
     }
     else { //mbedtls does not set point A for some curves
-        if (pointA[ecdsa->grp.id]) {
+        if (pointA[ecdsa->grp.id] && ecdsa->grp.id < 6) {
             *p++ = 0x82; p += format_tlv_len(p_size, p); memcpy(p, pointA[ecdsa->grp.id], p_size); p += p_size;
         }
         else {
-            *p++ = 0x82; p += format_tlv_len(0, p);
+            *p++ = 0x82; p += format_tlv_len(1, p);
+            *p++ = 0x0;
         }
     }
     //B
diff --git a/src/hsm/kek.c b/src/hsm/kek.c
index 93b53a2..f1f1317 100644
--- a/src/hsm/kek.c
+++ b/src/hsm/kek.c
@@ -27,10 +27,13 @@
 #include "mbedtls/cmac.h"
 #include "mbedtls/rsa.h"
 #include "mbedtls/ecdsa.h"
+#include "mbedtls/chachapoly.h"
 #include "files.h"
 
 extern bool has_session_pin, has_session_sopin;
 extern uint8_t session_pin[32], session_sopin[32];
+uint8_t mkek_mask[MKEK_KEY_SIZE];
+bool has_mkek_mask = false;
 
 #define POLY 0xedb88320
 
@@ -65,6 +68,12 @@ int load_mkek(uint8_t *mkek) {
     }
     if (pin == NULL) //Should never happen
         return CCID_EXEC_ERROR;
+
+    if (has_mkek_mask) {
+        for (int i = 0; i < MKEK_KEY_SIZE; i++) {
+            MKEK_KEY(mkek)[i] ^= mkek_mask[i];
+        }
+    }
     int ret = aes_decrypt_cfb_256(pin, MKEK_IV(mkek), MKEK_KEY(mkek), MKEK_KEY_SIZE+MKEK_KEY_CS_SIZE);
     if (ret != 0)
         return CCID_EXEC_ERROR;
@@ -73,6 +82,17 @@ int load_mkek(uint8_t *mkek) {
     return CCID_OK;
 }
 
+mse_t mse = {.init = false};
+
+int mse_decrypt_ct(uint8_t *data, size_t len) {
+    mbedtls_chachapoly_context chatx;
+    mbedtls_chachapoly_init(&chatx);
+    mbedtls_chachapoly_setkey(&chatx, mse.key_enc + 12);
+    int ret = mbedtls_chachapoly_auth_decrypt(&chatx, len - 16, mse.key_enc, mse.Qpt, 65, data + len - 16, data, data);
+    mbedtls_chachapoly_free(&chatx);
+    return ret;
+}
+
 int load_dkek(uint8_t id, uint8_t *dkek) {
     file_t *tf = search_dynamic_file(EF_DKEK+id);
     if (!tf)
diff --git a/src/hsm/kek.h b/src/hsm/kek.h
index 8a0e5d1..479aeb4 100644
--- a/src/hsm/kek.h
+++ b/src/hsm/kek.h
@@ -18,6 +18,8 @@
 #ifndef _DKEK_H_
 #define _DKEK_H_
 
+#include "crypto_utils.h"
+
 extern int load_mkek(uint8_t *);
 extern int store_mkek(const uint8_t *);
 extern int save_dkek_key(uint8_t, const uint8_t *key);
@@ -45,4 +47,16 @@ extern int dkek_decode_key(uint8_t, void *key_ctx, const uint8_t *in, size_t in_
 #define MKEK_CHECKSUM(p) (MKEK_KEY(p)+MKEK_KEY_SIZE)
 #define DKEK_KEY_SIZE    (32)
 
+extern uint8_t mkek_mask[MKEK_KEY_SIZE];
+extern bool has_mkek_mask;
+
+typedef struct mse {
+    uint8_t Qpt[65];
+    uint8_t key_enc[12 + 32];
+    bool init;
+} mse_t;
+extern mse_t mse;
+
+extern int mse_decrypt_ct(uint8_t *, size_t);
+
 #endif
diff --git a/src/hsm/sc_hsm.c b/src/hsm/sc_hsm.c
index feae664..ab45c41 100644
--- a/src/hsm/sc_hsm.c
+++ b/src/hsm/sc_hsm.c
@@ -647,7 +647,9 @@ static const cmd_t cmds[] = {
 };
 
 int sc_hsm_process_apdu() {
-    sm_unwrap();
+    int r = sm_unwrap();
+    if (r != CCID_OK)
+        return SW_DATA_INVALID();
     for (const cmd_t *cmd = cmds; cmd->ins != 0x00; cmd++) {
         if (cmd->ins == INS(apdu)) {
             int r = cmd->cmd_handler();
diff --git a/src/hsm/sc_hsm.h b/src/hsm/sc_hsm.h
index 45d590a..984473e 100644
--- a/src/hsm/sc_hsm.h
+++ b/src/hsm/sc_hsm.h
@@ -77,6 +77,7 @@ extern const uint8_t sc_hsm_aid[];
 #define HSM_OPT_RRC_RESET_ONLY      0x0020
 #define HSM_OPT_BOOTSEL_BUTTON      0x0100
 #define HSM_OPT_KEY_COUNTER_ALL     0x0200
+#define HSM_OPT_SECURE_LOCK         0x0400
 
 #define PRKD_PREFIX				0xC4		/* Hi byte in file identifier for PKCS#15 PRKD objects */
 #define CD_PREFIX				0xC8		/* Hi byte in file identifier for PKCS#15 CD objects */
diff --git a/tools/pico-hsm-tool.py b/tools/pico-hsm-tool.py
old mode 100755
new mode 100644
index 93d001e..00b399c
--- a/tools/pico-hsm-tool.py
+++ b/tools/pico-hsm-tool.py
@@ -33,10 +33,21 @@ try:
     from cvc.asn1 import ASN1
     from cvc.oid import oid2scheme
     from cvc.utils import scheme_rsa
-    from cryptography.hazmat.primitives.asymmetric import ec
 except ModuleNotFoundError:
     print('ERROR: cvc module not found! Install pycvc package.\nTry with `pip install pycvc`')
     sys.exit(-1)
+
+try:
+    from cryptography.hazmat.primitives.asymmetric import ec
+    from cryptography.hazmat.primitives.kdf.hkdf import HKDF
+    from cryptography.hazmat.primitives.serialization import Encoding, PublicFormat
+    from cryptography.hazmat.primitives.ciphers.aead import ChaCha20Poly1305
+    from cryptography.hazmat.primitives import hashes
+except ModuleNotFoundError:
+    print('ERROR: cryptography module not found! Install cryptography package.\nTry with `pip install cryptography`')
+    sys.exit(-1)
+
+
 import json
 import urllib.request
 import base64
@@ -44,9 +55,20 @@ from binascii import hexlify
 import sys
 import argparse
 import os
+import platform
 from datetime import datetime
 from argparse import RawTextHelpFormatter
 
+if (platform.system() == 'Windows'):
+    from secure_key import windows as skey
+elif (platform.system() == 'Linux'):
+    from secure_key import linux as skey
+elif (platform.system() == 'Darwin'):
+    from secure_key import macos as skey
+else:
+    print('ERROR: platform not supported')
+    sys.exit(-1)
+
 class APDUResponse(Exception):
     def __init__(self, sw1, sw2):
         self.sw1 = sw1
@@ -98,6 +120,9 @@ def parse_args():
     parser_opts.add_argument('opt', choices=['button', 'counter'], help='Button: press-to-confirm button.\nCounter: every generated key has an internal counter.')
     parser_opts.add_argument('onoff', choices=['on', 'off'], help='Toggles state ON or OFF', metavar='ON/OFF', nargs='?')
 
+    parser_secure = subparser.add_parser('secure', help='Manages security of Pico Fido.')
+    parser_secure.add_argument('subcommand', choices=['enable', 'disable', 'unlock'], help='Enables, disables or unlocks the security.')
+
     args = parser.parse_args()
     return args
 
@@ -276,8 +301,68 @@ def opts(card, args):
     elif (args.subcommand == 'get'):
         print(f'Option {args.opt.upper()} is {"ON" if current & opt else "OFF"}')
 
+class SecureLock:
+    def __init__(self, card):
+        self.card = card
+
+    def mse(self):
+        sk = ec.generate_private_key(ec.SECP256R1())
+        pn = sk.public_key().public_numbers()
+        self.__pb = sk.public_key().public_bytes(Encoding.X962, PublicFormat.UncompressedPoint)
+
+
+        ret = send_apdu(self.card, [0x80, 0x64], 0x3A, 0x01, list(self.__pb))
+
+        pk = ec.EllipticCurvePublicKey.from_encoded_point(ec.SECP256R1(), bytes(ret))
+        shared_key = sk.exchange(ec.ECDH(), pk)
+
+        xkdf = HKDF(
+            algorithm=hashes.SHA256(),
+            length=12+32,
+            salt=None,
+            info=self.__pb
+        )
+        kdf_out = xkdf.derive(shared_key)
+        self.__key_enc = kdf_out[12:]
+        self.__iv = kdf_out[:12]
+
+    def encrypt_chacha(self, data):
+        chacha = ChaCha20Poly1305(self.__key_enc)
+        ct = chacha.encrypt(self.__iv, data, self.__pb)
+        return ct
+
+    def unlock_device(self):
+        ct = self.get_skey()
+        send_apdu(self.card, [0x80, 0x64], 0x3A, 0x03, list(ct))
+
+    def _get_key_device(self):
+        return skey.get_secure_key()
+
+    def get_skey(self):
+        self.mse()
+        ct = self.encrypt_chacha(self._get_key_device())
+        return ct
+
+    def enable_device_aut(self):
+        ct = self.get_skey()
+        send_apdu(self.card, [0x80, 0x64], 0x3A, 0x02, list(ct))
+
+    def disable_device_aut(self):
+        ct = self.get_skey()
+        send_apdu(self.card, [0x80, 0x64], 0x3A, 0x04, list(ct))
+
+
+def secure(card, args):
+    slck = SecureLock(card)
+    if (args.subcommand == 'enable'):
+        slck.enable_device_aut()
+    elif (args.subcommand == 'unlock'):
+        slck.unlock_device()
+    elif (args.subcommand == 'disable'):
+        slck.disable_device_aut()
+
 def main(args):
-    print('Pico HSM Tool v1.4')
+    print('Pico HSM Tool v1.6')
     print('Author: Pol Henarejos')
     print('Report bugs to https://github.com/polhenarejos/pico-hsm/issues')
     print('')
@@ -305,6 +390,8 @@ def main(args):
         rtc(card, args)
     elif (args.command == 'options'):
         opts(card, args)
+    elif (args.command == 'secure'):
+        secure(card, args)
 
 def run():
     args = parse_args()