state: client cert/verify init

src/certificate.rs

@@ -553,7 +553,6 @@ impl<'a> Certificate<'a> {
     pub fn get_cert_public_key(&self) -> Result<CertificatePublicKey, ()> {
         let public_key_info = &self.tbs_certificate.subject_public_key_info;
         let algorithm_identifier = &public_key_info.algorithm;
-        log::info!("sig alg ident: {:?}", algorithm_identifier);
 
         // 3 possibilities: RSA_ENCRYPTION, ID_EC_PUBLIC_KEY, and EdDSA25519
         match algorithm_identifier.algorithm {
@@ -561,8 +560,6 @@ impl<'a> Certificate<'a> {
                 let (_, (modulus, exponent)) = parse_asn1_der_rsa_public_key(
                     self.tbs_certificate.subject_public_key_info.subject_public_key
                 ).map_err(|_| ())?;
-
-                log::info!("Mod: {:?}, exp: {:?}", modulus, exponent);
         
                 let public_key = RSAPublicKey::new(
                     BigUint::from_bytes_be(modulus),
@@ -616,7 +613,6 @@ impl<'a> Certificate<'a> {
     pub fn validate_self_signed_signature(&self) -> Result<(), TlsError> {
         let cert_public_key = self.get_cert_public_key()
             .map_err(|_| TlsError::SignatureValidationError)?;
-        log::info!("Own public key: {:?}", cert_public_key);
         self.validate_signature_with_trusted(&cert_public_key)
     }
 
@@ -630,7 +626,6 @@ impl<'a> Certificate<'a> {
         let sig_alg = self.signature_algorithm.algorithm;
 
         // Prepare hash value
-        log::info!("sig alg: {:?}", sig_alg);
         match sig_alg {
             SHA1_WITH_RSA_ENCRYPTION => {
                 let padding = PaddingScheme::new_pkcs1v15_sign(Some(Hash::SHA1));
@@ -686,7 +681,6 @@ impl<'a> Certificate<'a> {
                 let (_, (hash_alg, salt_len)) = parse_rsa_ssa_pss_parameters(
                     self.signature_algorithm.parameters
                 ).unwrap();
-                log::info!("Hash alg, salt_len: {:X?}, {:X?}", hash_alg, salt_len);
                 match hash_alg {
                     ID_SHA1 => {
                         let padding = PaddingScheme::new_pss_with_salt::<Sha1, FakeRandom>(
@@ -715,15 +709,12 @@ impl<'a> Certificate<'a> {
                     },
 
                     ID_SHA256 => {
-                        log::info!("Selected SHA256 with salt length: {:?}", salt_len);
                         let padding = PaddingScheme::new_pss_with_salt::<Sha256, FakeRandom>(
                             FakeRandom {},
                             salt_len
                         );
                         let hashed = Sha256::digest(self.tbs_certificate_encoded);
                         let sig = self.signature_value;
-                        log::info!("signature: {:X?}", sig);
-                        log::info!("Trusted key: {:?}", trusted_public_key);
                         trusted_public_key.get_rsa_public_key()
                             .map_err(|_| TlsError::SignatureValidationError)?
                             .verify(padding, &hashed, sig)
@@ -778,7 +769,6 @@ impl<'a> Certificate<'a> {
                 let sig = ed25519_dalek::Signature::try_from(
                     self.signature_value
                 ).map_err(|_| TlsError::SignatureValidationError)?;
-                log::info!("Ed25519 signature: {:?}", sig);
                 trusted_public_key.get_ed25519_public_key()
                     .map_err(|_| TlsError::SignatureValidationError)?
                     .verify_strict(self.tbs_certificate_encoded, &sig)

src/main.rs

@@ -68,6 +68,7 @@ fn main() {
             rx_buffer,
             tx_buffer,
             &mut RNG,
+            None
         )
     };
 

src/parse.rs

@@ -599,6 +599,36 @@ fn parse_extension(bytes: &[u8], handshake_type: HandshakeType) -> IResult<&[u8]
                     _ => todo!()
                 }
             },
+            SignatureAlgorithms => {
+                let (rest, supported_signature_algorithm_length) =
+                    take(2_usize)(rest)?;
+                let supported_signature_algorithm_length =
+                    NetworkEndian::read_u16(supported_signature_algorithm_length);
+                
+                // Take the allocated extension bytes from rest
+                let (rest, mut algorithms) = take(
+                    supported_signature_algorithm_length
+                )(rest)?;
+
+                // Parse all algorithms
+                let mut supported_signature_algorithms: Vec<SignatureScheme> =
+                    Vec::new();
+                while algorithms.len() != 0 {
+                    let (rem, algorithm) = take(2_usize)(algorithms)?;
+                    let sig_alg = SignatureScheme::try_from(
+                        NetworkEndian::read_u16(algorithm)
+                    ).unwrap();
+                    algorithms = rem;
+                    supported_signature_algorithms.push(sig_alg);
+                }
+
+                let signature_scheme_list = crate::tls_packet::SignatureSchemeList {
+                    length: supported_signature_algorithm_length,
+                    supported_signature_algorithms
+                };
+
+                (rest, ExtensionData::SignatureAlgorithms(signature_scheme_list))
+            }
             _ => todo!()
         }        
     };
@@ -1623,7 +1653,6 @@ pub fn parse_rsa_ssa_pss_parameters(params: &[u8]) -> IResult<&[u8], (&[u8], usi
     }
     
     // Parse as RSASSA-PSS-params (Sequence: 0x30)
-    log::info!("sig_alg sequence: {:X?}", params);
     let (_, rsa_ssa_params) = complete(
         parse_asn1_der_sequence
     )(params)?;
@@ -1637,8 +1666,6 @@ pub fn parse_rsa_ssa_pss_parameters(params: &[u8]) -> IResult<&[u8], (&[u8], usi
         ))
     )(rsa_ssa_params)?;
 
-    log::info!("Parser hash algorithm: {:?}", hash_alg);
-
     let hash_alg = hash_alg.unwrap_or(
         Asn1DerAlgId { algorithm: ID_SHA1, parameters: &[] }
     );
@@ -1657,7 +1684,7 @@ pub fn parse_rsa_ssa_pss_parameters(params: &[u8]) -> IResult<&[u8], (&[u8], usi
         todo!()
     }
     let mut array_buffer: [u8; core::mem::size_of::<usize>()] = [0; core::mem::size_of::<usize>()];
-    array_buffer[(8-salt_len.len())..].clone_from_slice(salt_len);
+    array_buffer[(core::mem::size_of::<usize>()-salt_len.len())..].clone_from_slice(salt_len);
     let salt_len = usize::from_be_bytes(array_buffer);
 
     Ok((
@@ -1671,7 +1698,6 @@ pub fn parse_rsa_ssa_pss_parameters(params: &[u8]) -> IResult<&[u8], (&[u8], usi
 
 fn parse_hash_algorithm(bytes: &[u8]) -> IResult<&[u8], Asn1DerAlgId> {
     // Parse HashAlgorithm [0]
-    log::info!("Hash algorithm: {:X?}", bytes);
     let (rest, (tag_val, _, hash_alg)) = parse_asn1_der_object(bytes)?;
     // Verify the tag is indeed 0xA0
     if tag_val != 0xA0 {
@@ -1679,7 +1705,6 @@ fn parse_hash_algorithm(bytes: &[u8]) -> IResult<&[u8], Asn1DerAlgId> {
     }
     // Parse the encapsulated algorithm identifier, force completeness
     let (_, hash_alg) = complete(parse_asn1_der_algorithm_identifier)(hash_alg)?;
-    log::info!("Parsed hash algorithm {:?}", hash_alg);
     Ok((
         rest, hash_alg
     ))

src/session.rs

@@ -1,4 +1,4 @@
-use p256::{ EncodedPoint, ecdh::EphemeralSecret };
+use p256::{ EncodedPoint, ecdh::EphemeralSecret, ecdsa::signature::DigestVerifier };
 use heapless::{ Vec, consts::* };
 use sha2::{ Digest, Sha256, Sha384, Sha512, digest::FixedOutput };
 use aes_gcm::{ Aes128Gcm, Aes256Gcm, aes::Aes128 };
@@ -18,9 +18,11 @@ use crate::tls_packet::SignatureScheme;
 use crate::Error;
 use crate::fake_rng::FakeRandom;
 
+use core::convert::TryFrom;
+
 type Aes128Ccm = Ccm<Aes128, U16, U12>;
 
-pub(crate) struct Session {
+pub(crate) struct Session<'a> {
     state: TlsState,
     role: TlsRole,
     // Session ID for this session
@@ -59,14 +61,22 @@ pub(crate) struct Session {
     // Reset to 0 on rekey AND key exchange
     // TODO: Force rekey if sequence number need to wrap (very low priority)
     client_sequence_number: u64,
-    server_sequence_number: u64,
+    pub server_sequence_number: u64,
     // Certificate public key
     // For Handling CertificateVerify
     cert_public_key: Option<CertificatePublicKey>,
+    // Client certificate and its private key
+    cert_private_key: Option<(CertificatePrivateKey, alloc::vec::Vec<&'a [u8]>)>,
+    // Flag for noting the need to send client certificate
+    // Client must cent Certificate extension iff server requested it
+    need_send_client_cert: bool,
+    client_cert_verify_sig_alg: Option<crate::tls_packet::SignatureScheme>,
 }
 
-impl Session {
+impl<'a> Session<'a> {
-    pub(crate) fn new(role: TlsRole) -> Self {
+    pub(crate) fn new(role: TlsRole, certificate_with_key: Option<(
+        CertificatePrivateKey, alloc::vec::Vec<&'a [u8]>
+    )>) -> Self {
         let hash = Hash::Undetermined {
             sha256: Sha256::new(),
             sha384: Sha384::new(),
@@ -93,7 +103,10 @@ impl Session {
             server_application_nonce: None,
             client_sequence_number: 0,
             server_sequence_number: 0,
-            cert_public_key: None
+            cert_public_key: None,
+            cert_private_key: certificate_with_key,
+            need_send_client_cert: false,
+            client_cert_verify_sig_alg: None
         }
     }
 
@@ -129,15 +142,7 @@ impl Session {
         if self.state != TlsState::WAIT_SH || self.role != TlsRole::Client {
             todo!()
         }
-        // Generate ECDHE shared secret
+       
-        // Remove private secret
-        // let ecdhe_shared_secret =
-        //     self.ecdhe_secret
-        //         .take()
-        //         .unwrap()
-        //         .diffie_hellman(&encoded_point)
-        //         .unwrap();
-        
         let mut shared_secret_bytes: [u8; 32] = [0; 32];
         if encoded_point.is_some() {
             let p256_shared_secret =
@@ -496,6 +501,72 @@ impl Session {
         self.state = TlsState::WAIT_CERT_CR;
     }
 
+    pub(crate) fn client_update_for_certificate_request(
+        &mut self,
+        cert_request_slice: &[u8],
+        signature_algorithms: &[crate::tls_packet::SignatureScheme]
+    ) {
+        self.hash.update(cert_request_slice);
+        // Note the need of sending client certificate
+        self.need_send_client_cert = true;
+        // Determine the supplied client certificate indeed has an
+        // acceptable signature algorithm
+        let mut private_key_algorithm_acceptable = false;
+        if let Some((private_key, cert)) = &self.cert_private_key {
+            if let CertificatePrivateKey::RSA {..} = private_key {
+                for sig_alg in signature_algorithms.iter() {
+                    use crate::tls_packet::SignatureScheme::*;
+                    if *sig_alg == rsa_pkcs1_sha256
+                        || *sig_alg == rsa_pkcs1_sha384
+                        || *sig_alg == rsa_pkcs1_sha512
+                        || *sig_alg == rsa_pss_rsae_sha256
+                        || *sig_alg == rsa_pss_rsae_sha384
+                        || *sig_alg == rsa_pss_rsae_sha512
+                        || *sig_alg == rsa_pss_pss_sha256
+                        || *sig_alg == rsa_pss_pss_sha384
+                        || *sig_alg == rsa_pss_pss_sha512
+                    {
+                        private_key_algorithm_acceptable = true;
+                        self.client_cert_verify_sig_alg.replace(*sig_alg);
+                        break;
+                    }
+                }
+            } else if let CertificatePrivateKey::ECDSA_SECP256R1_SHA256 {..}
+                = private_key
+            {
+                for sig_alg in signature_algorithms.iter() {
+                    use crate::tls_packet::SignatureScheme::*;
+                    if *sig_alg == ecdsa_secp256r1_sha256
+                    {
+                        private_key_algorithm_acceptable = true;
+                        self.client_cert_verify_sig_alg.replace(*sig_alg);
+                        break;
+                    }
+                }
+            } else if let CertificatePrivateKey::ED25519 {..} = private_key {
+                for sig_alg in signature_algorithms.iter() {
+                    use crate::tls_packet::SignatureScheme::*;
+                    if *sig_alg == ed25519
+                    {
+                        private_key_algorithm_acceptable = true;
+                        self.client_cert_verify_sig_alg.replace(*sig_alg);
+                        break;
+                    }
+                }
+            }
+        }
+
+        // Dump the private key and certificate if the other side will not take it
+        if !private_key_algorithm_acceptable {
+            self.cert_private_key.take();
+        }
+
+        log::info!("client key: {:?}", self.cert_private_key.is_some());
+
+        // Move to the next state
+        self.state = TlsState::WAIT_CERT;
+    }
+
     pub(crate) fn client_update_for_wait_cert_cr(
         &mut self,
         cert_slice: &[u8],
@@ -525,11 +596,39 @@ impl Session {
         // Handle Ed25519 and p256 separately
         // These 2 algorithms have a mandated hash function
         if signature_algorithm == SignatureScheme::ecdsa_secp256r1_sha256 {
-            todo!()
+            let verify_hash = Sha256::new()
+                .chain(&[0x20; 64])
+                .chain("TLS 1.3, server CertificateVerify")
+                .chain(&[0])
+                .chain(&transcript_hash);
+            let ecdsa_signature = p256::ecdsa::Signature::from_asn1(signature).unwrap();
+            self.cert_public_key
+                .take()
+                .unwrap()
+                .get_ecdsa_secp256r1_sha256_verify_key()
+                .unwrap()
+                .verify_digest(
+                    verify_hash, &ecdsa_signature
+                ).unwrap();
+            return
         }
 
         if signature_algorithm == SignatureScheme::ed25519 {
-            todo!()
+            let verify_hash = Sha512::new()
+                .chain(&[0x20; 64])
+                .chain("TLS 1.3, server CertificateVerify")
+                .chain(&[0])
+                .chain(&transcript_hash);
+            let ed25519_signature = ed25519_dalek::Signature::try_from(
+                signature
+            ).unwrap();
+            self.cert_public_key.take()
+                .unwrap()
+                .get_ed25519_public_key()
+                .unwrap()
+                .verify_prehashed(verify_hash, None, &ed25519_signature)
+                .unwrap();
+            return
         }
 
         // Get verification hash, and verify the signature
@@ -696,6 +795,8 @@ impl Session {
                 self.hash.get_sha256_clone().unwrap()
             );
 
+            log::info!("Server traffic secret: {:?}", server_application_traffic_secret);
+
             self.client_application_traffic_secret.replace(
                 Vec::from_slice(&client_application_traffic_secret).unwrap()
             );
@@ -983,6 +1084,26 @@ impl Session {
         self.state = TlsState::SERVER_CONNECTED;
     }
 
+    pub(crate) fn client_update_for_certificate_in_server_connected(
+        &mut self,
+        client_certificate_slice: &[u8]
+    )
+    {
+        // Sequence number is updated by send methods
+        // No need to change state as client will send everything needed immediately
+        self.hash.update(client_certificate_slice);
+    }
+
+    pub(crate) fn client_update_for_cert_verify_in_server_connected(
+        &mut self,
+        client_certificate_verify_slice: &[u8]
+    )
+    {
+        // Sequence number is updated by send methods
+        // No need to change state as client will send everything needed immediately
+        self.hash.update(client_certificate_verify_slice);
+    }
+
 
     pub(crate) fn client_update_for_server_connected(
         &mut self,
@@ -1017,6 +1138,190 @@ impl Session {
         self.changed_cipher_spec = true;
     }
 
+    pub(crate) fn need_to_send_client_certificate(&self) -> bool {
+        self.need_send_client_cert
+    }
+
+    pub(crate) fn get_private_certificate_slices(&self) -> Option<&alloc::vec::Vec<&[u8]>> {
+        if let Some((_, cert_vec)) = &self.cert_private_key {
+            Some(cert_vec)
+        } else {
+            None
+        }
+    }
+
+    pub(crate) fn get_client_certificate_verify_signature(&self)
+        -> (crate::tls_packet::SignatureScheme, alloc::vec::Vec<u8>) 
+    {
+        if let Some((private_key, client_certificate)) = &self.cert_private_key {
+            let transcript_hash: Vec<u8, U64> =
+                if let Ok(sha256) = self.hash.get_sha256_clone() {
+                    Vec::from_slice(&sha256.finalize()).unwrap()
+                } else if let Ok(sha384) = self.hash.get_sha384_clone() {
+                    Vec::from_slice(&sha384.finalize()).unwrap()
+                } else {
+                    unreachable!()
+                };
+            
+            use crate::tls_packet::SignatureScheme::*;
+            // RSA signature must be with PSS padding scheme
+            let get_rsa_padding_scheme = |sig_alg: SignatureScheme|
+                -> (SignatureScheme, PaddingScheme)
+            {
+                match sig_alg {
+                    rsa_pkcs1_sha256 => {
+                        (
+                            rsa_pss_rsae_sha256,
+                            PaddingScheme::new_pkcs1v15_sign(Some(RSAHash::SHA2_256))
+                        )
+                    },
+                    rsa_pkcs1_sha384 => {
+                        (
+                            rsa_pss_rsae_sha384,
+                            PaddingScheme::new_pkcs1v15_sign(Some(RSAHash::SHA2_384))
+                        )
+                    },
+                    rsa_pkcs1_sha512 => {
+                        (
+                            rsa_pss_rsae_sha512,
+                            PaddingScheme::new_pkcs1v15_sign(Some(RSAHash::SHA2_512))
+                        )
+                    },
+                    rsa_pss_rsae_sha256 | rsa_pss_pss_sha256 => {
+                        (
+                            sig_alg,
+                            PaddingScheme::new_pss::<Sha256, FakeRandom>(FakeRandom{})
+                        )
+                    },
+                    rsa_pss_rsae_sha384 | rsa_pss_pss_sha384 => {
+                        (
+                            sig_alg,
+                            PaddingScheme::new_pss::<Sha384, FakeRandom>(FakeRandom{})
+                        )
+                    },
+                    rsa_pss_rsae_sha512 | rsa_pss_pss_sha512 => {
+                        (
+                            sig_alg,
+                            PaddingScheme::new_pss::<Sha512, FakeRandom>(FakeRandom{})
+                        )
+                    },
+                    _ => unreachable!()
+                }
+            };
+
+            match private_key {
+                CertificatePrivateKey::RSA { cert_rsa_private_key } => {
+                    let sig_alg = self.client_cert_verify_sig_alg.unwrap();
+                    let verify_hash: Vec<u8, U64> = match sig_alg {
+                        rsa_pkcs1_sha256 | rsa_pss_rsae_sha256 | rsa_pss_pss_sha256 => {
+                            Vec::from_slice(
+                                &sha2::Sha256::new()
+                                    .chain(&[0x20; 64])
+                                    .chain("TLS 1.3, client CertificateVerify")
+                                    .chain(&[0x00])
+                                    .chain(&transcript_hash)
+                                    .finalize()
+                            ).unwrap()
+                        },
+                        rsa_pkcs1_sha384 | rsa_pss_rsae_sha384 | rsa_pss_pss_sha384 => {
+                            Vec::from_slice(
+                                &sha2::Sha384::new()
+                                    .chain(&[0x20; 64])
+                                    .chain("TLS 1.3, client CertificateVerify")
+                                    .chain(&[0x00])
+                                    .chain(&transcript_hash)
+                                    .finalize()
+                            ).unwrap()
+                        },
+                        rsa_pkcs1_sha512 | rsa_pss_rsae_sha512 | rsa_pss_pss_sha512 => {
+                            Vec::from_slice(
+                                &sha2::Sha512::new()
+                                    .chain(&[0x20; 64])
+                                    .chain("TLS 1.3, client CertificateVerify")
+                                    .chain(&[0x00])
+                                    .chain(&transcript_hash)
+                                    .finalize()
+                            ).unwrap()
+                        },
+                        _ => unreachable!()
+                    };
+                    let (modified_sig_alg, padding) = get_rsa_padding_scheme(sig_alg);
+                    (
+                        modified_sig_alg,
+                        cert_rsa_private_key.sign(
+                            padding, &verify_hash
+                        ).unwrap()
+                    )
+                },
+
+                CertificatePrivateKey::ECDSA_SECP256R1_SHA256 { cert_signing_key } => {
+                    let verify_hash = sha2::Sha256::new()
+                        .chain(&[0x20; 64])
+                        .chain("TLS 1.3, client CertificateVerify")
+                        .chain(&[0x00])
+                        .chain(&transcript_hash);
+                    
+                    use p256::ecdsa::signature::DigestSigner;
+                    let sig_vec = alloc::vec::Vec::from(
+                        cert_signing_key.sign_digest(verify_hash).as_ref()
+                    );
+
+                    (
+                        ecdsa_secp256r1_sha256,
+                        sig_vec
+                    )
+                },
+
+                CertificatePrivateKey::ED25519 { cert_eddsa_key } => {
+                    let verify_hash = sha2::Sha512::new()
+                        .chain(&[0x20; 64])
+                        .chain("TLS 1.3, client CertificateVerify")
+                        .chain(&[0x00])
+                        .chain(&transcript_hash);
+                    
+                    // Ed25519 requires a key-pair to sign
+                    // Get public key from certificate
+                    let certificate = crate::parse::parse_asn1_der_certificate(
+                        client_certificate[0]
+                    ).unwrap().1;
+
+                    let cert_public_key = certificate
+                        .get_cert_public_key()
+                        .unwrap();
+                    let ed25519_public_key = cert_public_key
+                        .get_ed25519_public_key()
+                        .unwrap();
+
+                    let mut keypair_bytes: [u8; 64] = [0; 64];
+                    &keypair_bytes[..32].clone_from_slice(cert_eddsa_key.as_bytes());
+                    &keypair_bytes[32..].clone_from_slice(ed25519_public_key.as_bytes());
+
+                    let ed25519_keypair = ed25519_dalek::Keypair::from_bytes(
+                        &keypair_bytes
+                    ).unwrap();
+
+                    let sig_vec = alloc::vec::Vec::from(
+                        ed25519_keypair
+                            .sign_prehashed(verify_hash, None)
+                            .unwrap()
+                            .as_ref()
+                    );
+
+                    (
+                        ed25519,
+                        sig_vec
+                    )
+                }
+            }
+        }
+        else {
+            // Should definitely NOT be invoking this function
+            // TODO: Throw error
+            todo!()
+        }
+
+    }
+
     pub(crate) fn get_client_finished_verify_data(&self) -> Vec<u8, U64> {
         if let Ok(sha256) = self.hash.get_sha256_clone() {
             let hkdf = Hkdf::<Sha256>::from_prk(
@@ -1235,6 +1540,7 @@ impl Session {
         associated_data: &[u8],
         buffer: &mut [u8]
     ) -> Result<(), Error> {
+
         let (seq_num, nonce, cipher): (u64, &Vec<u8, U12>, &Cipher) = match self.role {
             TlsRole::Server => {(
                 self.client_sequence_number,
@@ -1519,3 +1825,15 @@ impl CertificatePublicKey {
         }
     }
 }
+
+pub enum CertificatePrivateKey {
+    RSA {
+        cert_rsa_private_key: rsa::RSAPrivateKey
+    },
+    ECDSA_SECP256R1_SHA256 {
+        cert_signing_key: p256::ecdsa::SigningKey
+    },
+    ED25519 {
+        cert_eddsa_key: ed25519_dalek::SecretKey
+    }
+}

src/tls.rs

@@ -14,6 +14,7 @@ use byteorder::{ByteOrder, NetworkEndian};
 use generic_array::GenericArray;
 
 use core::convert::TryFrom;
+use core::convert::TryInto;
 use core::cell::RefCell;
 
 use rand_core::{RngCore, CryptoRng};
@@ -52,19 +53,23 @@ pub(crate) enum TlsState {
 }
 
 // TODO: Group up all session_specific parameters into a separate structure
-pub struct TlsSocket<R: 'static + RngCore + CryptoRng>
+pub struct TlsSocket<'s, R: RngCore + CryptoRng>
 {
     tcp_handle: SocketHandle,
     rng: R,
-    session: RefCell<Session>,
+    session: RefCell<Session<'s>>,
 }
 
-impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
+impl<'s, R: RngCore + CryptoRng> TlsSocket<'s, R> {
     pub fn new<'a, 'b, 'c>(
         sockets: &mut SocketSet<'a, 'b, 'c>,
         rx_buffer: TcpSocketBuffer<'b>,
         tx_buffer: TcpSocketBuffer<'b>,
         rng: R,
+        certificate_with_key: Option<(
+            crate::session::CertificatePrivateKey,
+            Vec<&'s [u8]>
+        )>
     ) -> Self
     where
         'b: 'c,
@@ -75,7 +80,7 @@ impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
             tcp_handle,
             rng,
             session: RefCell::new(
-                Session::new(TlsRole::Client)
+                Session::new(TlsRole::Client, certificate_with_key)
             ),
         }
     }
@@ -162,10 +167,14 @@ impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
             // TLS client should jump from WAIT_SH directly to WAIT_CERT_CR directly.
             TlsState::WAIT_EE => {},
 
-            // TLS Client wait for server's certificate
+            // TLS Client wait for server's certificate/ certificate request
             // No need to send anything
             TlsState::WAIT_CERT_CR => {},
 
+            // TLS Client wait for server's certificate after receiveing a request
+            // No need to send anything
+            TlsState::WAIT_CERT => {},
+
             // TLS Client wait for server's certificate cerify
             // No need to send anything
             TlsState::WAIT_CV => {},
@@ -176,7 +185,148 @@ impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
             TlsState::WAIT_FINISHED => {}
 
             // Send client Finished to end handshake
+            // Also send certificate and certificate verify before client Finished if
+            // server sent a CertificateRequest beforehand
             TlsState::SERVER_CONNECTED => {
+                // Certificate & CertificateVerify
+                let need_to_send_client_cert = {
+                    self.session.borrow().need_to_send_client_certificate()
+                };
+                if need_to_send_client_cert {
+                    let (certificates_total_length, mut buffer_vec) = {
+                        let mut session = self.session.borrow_mut();
+                        let mut buffer_vec: Vec<u8> = Vec::new();
+                        let certificates = session
+                            .get_private_certificate_slices()
+                            .clone();
+    
+                        // Handshake level, client certificate byte followed by length (u24)
+                        let mut handshake_header: [u8; 4] = [11, 0, 0, 0];
+                        // Certificate struct:
+                        // request_context = X509: 0 (u8),
+                        // certificate_list to be determined (u24)
+                        let mut certificate_header: [u8; 4] = [0, 0, 0, 0];
+                        let mut certificates_total_length: u32 = 0;
+    
+                        // Append place holder bytes (8 of them) in the buffer vector
+                        // Simpily copy the the headers back into the vector
+                        // when all certificates are appended into the vector
+                        buffer_vec.extend_from_slice(&[11, 0, 0, 0, 0, 0, 0, 0]);
+    
+                        // Certificate Entry struct(s)
+                        if let Some(certificate_list) = certificates {
+                            for cert in certificate_list.iter() {
+                                // cert_data length, to be determined (u24)
+                                let mut cert_data_length: [u8; 3] = [0, 0, 0];
+                                // extensions: no extension needed
+                                let extension: [u8; 2] = [0, 0];
+    
+                                let certificate_length: u32 = u32::try_from(cert.len()).unwrap();
+    
+                                NetworkEndian::write_u24(
+                                    &mut cert_data_length,
+                                    certificate_length
+                                );
+    
+                                // Update length in Certificate struct
+                                certificates_total_length += (
+                                    // cert_data (len & data) AND extension (len & data)
+                                    3 + certificate_length + 2 + 0
+                                );
+    
+                                buffer_vec.extend_from_slice(&cert_data_length);
+                                buffer_vec.extend_from_slice(cert);
+                                buffer_vec.extend_from_slice(&extension);
+                            }
+                        }
+    
+                        // Write total certificate length into Certificate struct
+                        NetworkEndian::write_u24(
+                            &mut buffer_vec[5..8],
+                            certificates_total_length
+                        );
+    
+                        // Write the length of the entire handshake
+                        NetworkEndian::write_u24(
+                            &mut buffer_vec[1..4],
+                            // 4 bytes for the Certificate struct header
+                            certificates_total_length + 4
+                        );
+    
+                        // Inner plaintext: record type 
+                        buffer_vec.push(22);
+                        (certificates_total_length, buffer_vec)
+                    };
+
+                    self.send_application_slice(sockets, &mut buffer_vec.clone())?;
+                    // Update session
+                    let buffer_vec_length = buffer_vec.len();
+
+                    {
+                        self.session.borrow_mut()
+                            .client_update_for_certificate_in_server_connected(
+                            &buffer_vec[..(buffer_vec_length-1)]
+                        );
+                    }
+
+                    // Send a CertificateVerify as well if any certificates
+                    // were just sent by the client
+                    if certificates_total_length != 0 {
+                        // Serialize CertificateVerify
+
+                        // Handshake bytes:
+                        // msg_type = 15, CertificateVerify (u8)
+                        // handshake_data_length = to be determined (u24)
+                        // signature algorithm (u16)
+                        // signature_length (u16)
+                        // signature, the rest
+
+                        let mut verify_buffer_vec: Vec<u8> = Vec::new();
+                        // Leave bytes from Handshake struct as placeholders
+                        verify_buffer_vec.extend_from_slice(&[
+                            15,
+                            0, 0, 0,
+                            0, 0,
+                            0, 0
+                        ]);
+                        {
+                            let session = self.session.borrow();
+                            let (sig_alg, signature) = session
+                                .get_client_certificate_verify_signature();
+                            
+                            let signature_length: u16 = u16::try_from(signature.len()).unwrap();
+                            NetworkEndian::write_u24(
+                                &mut verify_buffer_vec[1..4],
+                                (signature_length + 4).into()
+                            );
+                            NetworkEndian::write_u16(
+                                &mut verify_buffer_vec[4..6],
+                                sig_alg.try_into().unwrap()
+                            );
+                            NetworkEndian::write_u16(
+                                &mut verify_buffer_vec[6..8],
+                                signature_length
+                            );
+                            verify_buffer_vec.extend_from_slice(&signature);
+                        }
+                        // Push content byte (handshake: 22)
+                        verify_buffer_vec.push(22);
+
+                        log::info!("Client CV: {:?}", verify_buffer_vec);
+                        self.send_application_slice(sockets, &mut verify_buffer_vec.clone())?;
+                        // Update session
+                        let cert_verify_len = verify_buffer_vec.len();
+
+                        {
+                            self.session.borrow_mut()
+                                .client_update_for_cert_verify_in_server_connected(
+                                    &verify_buffer_vec[..(cert_verify_len-1)]
+                                );
+                        }
+                    }
+                }
+
+                // Client Finished
                 let inner_plaintext: HeaplessVec<u8, U64> = {
                     let verify_data = self.session.borrow()
                         .get_client_finished_verify_data();
@@ -192,8 +342,10 @@ impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
                     buffer
                 };
                 self.send_application_slice(sockets, &mut inner_plaintext.clone())?;
+
+                let inner_plaintext_length = inner_plaintext.len();
                 self.session.borrow_mut()
-                    .client_update_for_server_connected(&inner_plaintext);
+                    .client_update_for_server_connected(&inner_plaintext[..(inner_plaintext_length-1)]);
             }
 
             _ => todo!()
@@ -449,38 +601,131 @@ impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
             // In this stage, wait for a certificate from server
             // Parse the certificate and check its content
             TlsState::WAIT_CERT_CR => {
-                // Verify that it is indeed an Certificate
+                // Verify that it is indeed an Certificate, or CertificateRequest
                 let might_be_cert = repr.handshake.take().unwrap();
-                if might_be_cert.get_msg_type() != HandshakeType::Certificate {
+                if might_be_cert.get_msg_type() == HandshakeType::Certificate {
-                    // Process the other handshakes in "handshake_vec"
+                    // Process certificates
+
+                    // let all_certificates = might_be_cert.get_all_asn1_der_certificates().unwrap();
+                    // log::info!("Number of certificates: {:?}", all_certificates.len());
+                    // log::info!("All certificates: {:?}", all_certificates);
+
+                    // TODO: Process all certificates
+                    let cert = might_be_cert.get_asn1_der_certificate().unwrap();
+
+                    // TODO: Replace this block after implementing a proper 
+                    // certificate verification procdeure
+                    cert.validate_self_signed_signature().expect("Signature mismatched");
+
+                    // Update session TLS state to WAIT_CV
+                    // Length of handshake header is 4
+                    let (_handshake_slice, cert_slice) = 
+                        take::<_, _, (&[u8], ErrorKind)>(
+                            might_be_cert.length + 4
+                        )(handshake_slice)
+                            .map_err(|_| Error::Unrecognized)?;
+
+                    self.session.borrow_mut()
+                        .client_update_for_wait_cert_cr(
+                            &cert_slice,
+                            cert.get_cert_public_key().unwrap()
+                        );
+                    log::info!("Received WAIT_CERT_CR");
+                }
+                else if might_be_cert.get_msg_type() == HandshakeType::CertificateRequest {
+                    // Process signature algorithm extension
+                    // Signature algorithm for the private key of client cert must be included
+                    // within the list of signature algorithms
+                    //
+                    // Client is STRONGLY RECOMMENDED to use a signature algorithm within
+                    // the list of `mandatory to implement` signature algorithms, including:
+                    // rsa_pkcs1_sha256, rsa_pss_rsae_sha256, ecdsa_secp256r1_sha256
+                    //
+                    // Update client state
+                    let cert_req_extensions: &Vec<Extension> = might_be_cert
+                        .get_cert_request_extensions()
+                        .unwrap();
+                    let sig_algs_ext: Option<&crate::tls_packet::Extension>
+                        = cert_req_extensions
+                            .iter()
+                            .find( |&extension| {
+                                extension.extension_type
+                                    == crate::tls_packet::ExtensionType::SignatureAlgorithms
+                                }
+                            );
+                    if sig_algs_ext.is_some() {
+                        // Convert extension into SignatureScheme
+                        if let crate::tls_packet::ExtensionData::SignatureAlgorithms(
+                            scheme_list
+                        ) = &sig_algs_ext.unwrap().extension_data {
+                            // Update session TLS state to WAIT_CERT
+                            // Length of handshake header is 4
+                            let (_handshake_slice, cert_req_slice) = 
+                                take::<_, _, (&[u8], ErrorKind)>(
+                                    might_be_cert.length + 4
+                                )(handshake_slice)
+                                    .map_err(|_| Error::Unrecognized)?;
+                            
+                            self.session.borrow_mut()
+                                .client_update_for_certificate_request(
+                                    &cert_req_slice,
+                                    &scheme_list.supported_signature_algorithms
+                                );
+                        }
+
+
+                    } else {
+                        // Reject connection, CertificateRequest must have
+                        // SignatureAlgorithm extension
+                        todo!()
+                    }
+                    log::info!("Received WAIT_CERT_CR");
+                }
+                else {
+                    // Throw alert
                     todo!()
                 }
+            },
 
-                // let all_certificates = might_be_cert.get_all_asn1_der_certificates().unwrap();
+            // In this stage, server will send a certificate chain
-                // log::info!("Number of certificates: {:?}", all_certificates.len());
+            // Verify the certificate
-                // log::info!("All certificates: {:?}", all_certificates);
+            TlsState::WAIT_CERT => {
+                // Verify that it is indeed an Certificate
+                let might_be_cert = repr.handshake.take().unwrap();
 
-                // TODO: Process all certificates
+                if might_be_cert.get_msg_type() == HandshakeType::Certificate {
-                let cert = might_be_cert.get_asn1_der_certificate().unwrap();
+                    // Process certificates
 
-                // TODO: Replace this block after implementing a proper 
+                    // let all_certificates = might_be_cert.get_all_asn1_der_certificates().unwrap();
-                // certificate verification procdeure
+                    // log::info!("Number of certificates: {:?}", all_certificates.len());
-                cert.validate_self_signed_signature().expect("Signature mismatched");
+                    // log::info!("All certificates: {:?}", all_certificates);
 
-                // Update session TLS state to WAIT_CV
+                    // TODO: Process all certificates
-                // Length of handshake header is 4
+                    let cert = might_be_cert.get_asn1_der_certificate().unwrap();
-                let (_handshake_slice, cert_slice) = 
-                    take::<_, _, (&[u8], ErrorKind)>(
-                        might_be_cert.length + 4
-                    )(handshake_slice)
-                        .map_err(|_| Error::Unrecognized)?;
 
-                self.session.borrow_mut()
+                    // TODO: Replace this block after implementing a proper 
-                    .client_update_for_wait_cert_cr(
+                    // certificate verification procdeure
-                        &cert_slice,
+                    cert.validate_self_signed_signature().expect("Signature mismatched");
-                        cert.get_cert_public_key().unwrap()
+
-                    );
+                    // Update session TLS state to WAIT_CV
-                log::info!("Received WAIT_CERT_CR");
+                    // Length of handshake header is 4
+                    let (_handshake_slice, cert_slice) = 
+                        take::<_, _, (&[u8], ErrorKind)>(
+                            might_be_cert.length + 4
+                        )(handshake_slice)
+                            .map_err(|_| Error::Unrecognized)?;
+
+                    self.session.borrow_mut()
+                        .client_update_for_wait_cert_cr(
+                            &cert_slice,
+                            cert.get_cert_public_key().unwrap()
+                        );
+                    log::info!("Received WAIT_CERT");
+                } else {
+                    // Unexpected handshakes
+                    // Throw alert
+                    todo!()
+                }
             },
 
             // In this stage, server will eventually send a CertificateVerify
@@ -619,6 +864,7 @@ impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
             },
             _ => return Err(Error::Illegal),
         };
+
         NetworkEndian::write_u16(
             &mut associated_data[3..5],
             auth_tag_length + u16::try_from(slice.len()).unwrap()
@@ -671,7 +917,34 @@ impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
             return Ok(0);
         }
 
-        let recv_slice_size = tcp_socket.recv_slice(data)?;
+        // TODO: Use `recv` to receive instead
+        // Issue with using recv slice:
+        // Encrypted application data can cramp together into a TCP Segment
+        // Dequeuing all bytes from the buffer immediately can cause
+        // 1. Incorrect decryption, hence throwing error, and
+        // 2. sequence number to go out of sync forever
+        let (recv_slice_size, acceptable) = tcp_socket.recv(
+            |buffer| {
+                // Read the size of the TLS record beforehand
+                let record_length: usize = NetworkEndian::read_u16(&buffer[3..5]).into();
+                let provided_data_capacity: usize = data.len();
+                // Copy the entire byte representation of TLS packet into the
+                // user-provided buffer, if possible
+                if provided_data_capacity >= (record_length + 5) {
+                    &data[..(record_length + 5)].clone_from_slice(&buffer[..(record_length + 5)]);
+                }
+                (
+                    (record_length + 5),
+                    (
+                        (record_length + 5),
+                        provided_data_capacity < (record_length + 5)
+                    )
+                )
+            }
+        )?;
+
+        // let recv_slice_size = tcp_socket.recv_slice(data)?;
+
         // Encrypted data need a TLS record wrapper (5 bytes)
         // Authentication tag (16 bytes, for all supported AEADs)
         // Content type byte (1 byte)
@@ -684,11 +957,12 @@ impl<R: 'static + RngCore + CryptoRng> TlsSocket<R> {
         let mut session = self.session.borrow_mut();
         let mut associated_data: [u8; 5] = [0; 5];
         associated_data.clone_from_slice(&data[..5]);
-        log::info!("Received encrypted appdata: {:?}", &data[..recv_slice_size]);
+        // log::info!("Received encrypted appdata: {:?}", &data[..recv_slice_size]);
 
         // Dump association data (TLS Record wrapper)
         // Only decrypt application data
         // Always increment sequence number after decrpytion
+        log::info!("Sequence number: {:?}", session.server_sequence_number);
         session.decrypt_application_data_in_place(
             &associated_data,
             &mut data[5..recv_slice_size]

src/tls_packet.rs

@@ -216,6 +216,17 @@ impl<'a, 'b> HandshakeRepr<'a> {
             Err(())
         }
     }
+
+    pub(crate) fn get_cert_request_extensions(&self) -> Result<&Vec<Extension>, ()> {
+        if self.msg_type != HandshakeType::CertificateRequest {
+            return Err(())
+        };
+        if let HandshakeData::CertificateRequest(req) = &self.handshake_data {
+            Ok(&req.extensions)
+        } else {
+            Err(())
+        }
+    }
 }
 
 #[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)]
@@ -620,6 +631,10 @@ pub enum SignatureScheme {
     rsa_pss_pss_sha256 = 0x0809,
     rsa_pss_pss_sha384 = 0x080a,
     rsa_pss_pss_sha512 = 0x080b,
+
+    /* Bad value */
+    #[num_enum(default)]
+    illegal = 0xFFFF,
 }
 
 #[derive(Debug, Clone)]