diff --git a/src/sm9_z256_alg.c b/src/sm9_z256_alg.c
index b3266aa4..64c8c19d 100644
--- a/src/sm9_z256_alg.c
+++ b/src/sm9_z256_alg.c
@@ -1746,7 +1746,6 @@ void sm9_z256_point_mul_generator(SM9_Z256_POINT *R, const sm9_z256_t k)
 	sm9_z256_point_mul(R, k, SM9_Z256_MONT_P1);
 }
 
-
 int sm9_z256_point_print(FILE *fp, int fmt, int ind, const char *label, const SM9_Z256_POINT *P)
 {
 	uint8_t buf[65];
diff --git a/src/sm9_z256_key.c b/src/sm9_z256_key.c
index 23927ae6..a5a20959 100644
--- a/src/sm9_z256_key.c
+++ b/src/sm9_z256_key.c
@@ -424,6 +424,30 @@ int sm9_enc_master_key_extract_key(SM9_ENC_MASTER_KEY *msk, const char *id, size
 	return 1;
 }
 
+int sm9_exch_master_key_extract_key(SM9_EXCH_MASTER_KEY *msk, const char *id, size_t idlen,
+	SM9_EXCH_KEY *key)
+{
+	sm9_z256_t t;
+
+	// t1 = H1(ID || hid, N) + ke
+	sm9_z256_hash1(t, id, idlen, SM9_HID_EXCH);
+	sm9_z256_fn_add(t, t, msk->ke);
+	if (sm9_z256_fn_is_zero(t)) {
+		error_print();
+		return -1;
+	}
+
+	// t2 = ke * t1^-1
+	sm9_z256_fn_inv(t, t);
+	sm9_z256_fn_mul(t, t, msk->ke);
+
+	// de = t2 * P2
+	sm9_z256_twist_point_mul_generator(&key->de, t);
+	key->Ppube = msk->Ppube;
+
+	return 1;
+}
+
 
 #define OID_SM9	oid_sm_algors,302
 static uint32_t oid_sm9[] = { OID_SM9 };
diff --git a/src/sm9_z256_lib.c b/src/sm9_z256_lib.c
index c4889d9e..207b6737 100644
--- a/src/sm9_z256_lib.c
+++ b/src/sm9_z256_lib.c
@@ -514,3 +514,162 @@ int sm9_decrypt(const SM9_ENC_KEY *key, const char *id, size_t idlen,
 	}
 	return 1;
 }
+
+
+
+int sm9_exch_step_1A(const SM9_EXCH_MASTER_KEY *mpk, const char *idB, size_t idBlen, SM9_Z256_POINT *RA, sm9_z256_t rA)
+{
+	// A1: Q = H1(ID_B||hid,N) * P1 + Ppube
+	sm9_z256_hash1(rA, idB, idBlen, SM9_HID_EXCH);
+	sm9_z256_point_mul(RA, rA, SM9_Z256_MONT_P1);
+	sm9_z256_point_add(RA, RA, &mpk->Ppube);
+
+	// A2: rand rA in [1, N-1]
+	if (sm9_z256_fn_rand(rA) != 1) {
+		error_print();
+		return -1;
+	}
+	// Only for testing
+	sm9_z256_from_hex(rA, "00005879DD1D51E175946F23B1B41E93BA31C584AE59A426EC1046A4D03B06C8");
+
+	// A3: RA = rA * Q
+	sm9_z256_point_mul(RA, rA, RA);
+
+	// A4: Output RA, save rA
+	return 1;
+}
+
+int sm9_exch_step_1B(const SM9_EXCH_MASTER_KEY *mpk, const char *idA, size_t idAlen, const char *idB, size_t idBlen,
+	const SM9_EXCH_KEY *key, const SM9_Z256_POINT *RA, SM9_Z256_POINT *RB, uint8_t *sk, size_t klen)
+{
+	sm9_z256_t rB;
+	sm9_z256_fp12 G1, G2, G3;
+	uint8_t g1[32 * 12], g2[32 * 12], g3[32 * 12];
+	uint8_t ta[65], tb[65];
+	SM3_KDF_CTX kdf_ctx;
+
+	// B1: Q = H1(ID_A||hid,N) * P1 + Ppube
+	sm9_z256_hash1(rB, idA, idAlen, SM9_HID_EXCH);
+	sm9_z256_point_mul(RB, rB, SM9_Z256_MONT_P1);
+	sm9_z256_point_add(RB, RB, &mpk->Ppube);
+
+	do {
+		// B2: rand rB in [1, N-1]
+		if (sm9_z256_fn_rand(rB) != 1) {
+			error_print();
+			return -1;
+		}
+		// Only for testing
+		sm9_z256_from_hex(rB, "00018B98C44BEF9F8537FB7D071B2C928B3BC65BD3D69E1EEE213564905634FE");
+
+		// B3: RB = rB * Q
+		sm9_z256_point_mul(RB, rB, RB);
+
+		// B4: check RA on curve; G1 = e(RA, deB), G2 = e(Ppube, P2) ^ rB, G3 = G1 ^ rB
+		if (!sm9_z256_point_is_on_curve(RA)) {
+			error_print();
+			return -1;
+		}
+		sm9_z256_pairing(G1, &key->de, RA);
+		sm9_z256_pairing(G2, SM9_Z256_MONT_P2, &mpk->Ppube);
+		sm9_z256_fp12_pow(G2, G2, rB);
+		sm9_z256_fp12_pow(G3, G1, rB);
+
+		sm9_z256_point_to_uncompressed_octets(RA, ta);
+		sm9_z256_point_to_uncompressed_octets(RB, tb);
+		sm9_z256_fp12_to_bytes(G1, g1);
+		sm9_z256_fp12_to_bytes(G2, g2);
+		sm9_z256_fp12_to_bytes(G3, g3);
+
+		// B5: sk = KDF(ID_A || ID_B || RA || RB || g1 || g2 || g3, klen)
+		sm3_kdf_init(&kdf_ctx, klen);
+		sm3_kdf_update(&kdf_ctx, idA, idAlen);
+		sm3_kdf_update(&kdf_ctx, idB, idBlen);
+		sm3_kdf_update(&kdf_ctx, ta + 1, 64);
+		sm3_kdf_update(&kdf_ctx, tb + 1, 64);
+		sm3_kdf_update(&kdf_ctx, g1, sizeof(g1));
+		sm3_kdf_update(&kdf_ctx, g2, sizeof(g2));
+		sm3_kdf_update(&kdf_ctx, g3, sizeof(g3));
+		sm3_kdf_finish(&kdf_ctx, sk);
+
+	} while (mem_is_zero(sk, klen) == 1);
+
+	// B6: SB = Hash(0x82 || g1 || Hash(g2 || g3 || ID_A || ID_B || RA || RB)) [optional]
+
+	gmssl_secure_clear(&rB, sizeof(rB));
+	gmssl_secure_clear(&G1, sizeof(G1));
+	gmssl_secure_clear(&G2, sizeof(G2));
+	gmssl_secure_clear(&G3, sizeof(G3));
+	gmssl_secure_clear(g1, sizeof(g1));
+	gmssl_secure_clear(g2, sizeof(g2));
+	gmssl_secure_clear(g3, sizeof(g3));
+	gmssl_secure_clear(ta, sizeof(ta));
+	gmssl_secure_clear(tb, sizeof(tb));
+	gmssl_secure_clear(&kdf_ctx, sizeof(kdf_ctx));
+
+	// B7: Output RB
+	return 1;
+}
+
+int sm9_exch_step_2A(const SM9_EXCH_MASTER_KEY *mpk, const char *idA, size_t idAlen, const char *idB, size_t idBlen,
+	const SM9_EXCH_KEY *key, const sm9_z256_t rA, const SM9_Z256_POINT *RA, const SM9_Z256_POINT *RB, uint8_t *sk, size_t klen)
+{
+	sm9_z256_t r;
+	sm9_z256_fp12 G1, G2, G3;
+	uint8_t g1[32 * 12], g2[32 * 12], g3[32 * 12];
+	uint8_t ta[65], tb[65];
+	SM3_KDF_CTX kdf_ctx;
+
+	do {
+		// A5: check RB on curve; G1 = e(Ppube, P2) ^ rA, G2 = e(RB, deA),  G3 = G2 ^ rA
+		if (!sm9_z256_point_is_on_curve(RB)) {
+			error_print();
+			return -1;
+		}
+		sm9_z256_pairing(G1, SM9_Z256_MONT_P2, &mpk->Ppube);
+		sm9_z256_fp12_pow(G1, G1, rA);
+		sm9_z256_pairing(G2, &key->de, RB);
+		sm9_z256_fp12_pow(G3, G2, rA);
+
+		sm9_z256_point_to_uncompressed_octets(RA, ta);
+		sm9_z256_point_to_uncompressed_octets(RB, tb);
+		sm9_z256_fp12_to_bytes(G1, g1);
+		sm9_z256_fp12_to_bytes(G2, g2);
+		sm9_z256_fp12_to_bytes(G3, g3);
+
+		// A6: S1 = Hash(0x82 || g1 || Hash(g2 || g3 || ID_A || ID_B || RA || RB)), check S1 = SB [optional]
+
+		// A7: sk = KDF(ID_A || ID_B || RA || RB || g1 || g2 || g3, klen) 
+		sm3_kdf_init(&kdf_ctx, klen);
+		sm3_kdf_update(&kdf_ctx, idA, idAlen);
+		sm3_kdf_update(&kdf_ctx, idB, idBlen);
+		sm3_kdf_update(&kdf_ctx, ta + 1, 64);
+		sm3_kdf_update(&kdf_ctx, tb + 1, 64);
+		sm3_kdf_update(&kdf_ctx, g1, sizeof(g1));
+		sm3_kdf_update(&kdf_ctx, g2, sizeof(g2));
+		sm3_kdf_update(&kdf_ctx, g3, sizeof(g3));
+		sm3_kdf_finish(&kdf_ctx, sk);
+
+	} while (mem_is_zero(sk, klen) == 1);
+
+	// A8: SA = Hash(0x83 || g1 || Hash(g2 || g3 || ID_A || ID_B || RA || RB)) [optional]
+
+	gmssl_secure_clear(&r, sizeof(r));
+	gmssl_secure_clear(&G1, sizeof(G1));
+	gmssl_secure_clear(&G2, sizeof(G2));
+	gmssl_secure_clear(&G3, sizeof(G3));
+	gmssl_secure_clear(g1, sizeof(g1));
+	gmssl_secure_clear(g2, sizeof(g2));
+	gmssl_secure_clear(g3, sizeof(g3));
+	gmssl_secure_clear(ta, sizeof(ta));
+	gmssl_secure_clear(tb, sizeof(tb));
+	gmssl_secure_clear(&kdf_ctx, sizeof(kdf_ctx));
+
+	return 1;
+}
+
+int sm9_exch_step_2B()
+{
+	// B8: S2 = Hash(0x83 || g1 || Hash(g2 || g3 || ID_A || ID_B || RA || RB)), check S2 = SA [optional]
+	return 1;
+}