diff --git a/Cargo.toml b/Cargo.toml
index 7830b03..582530e 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,6 +1,6 @@
 [package]
 name = "dht-pio"
-version = "0.1.0"
+version = "0.2.0"
 edition = "2021"
 
 license = "MIT"
diff --git a/readme.md b/readme.md
index 937daa8..e6c02bc 100644
--- a/readme.md
+++ b/readme.md
@@ -26,8 +26,8 @@ For the moment, the crates have only been tested on a Raspberry Pico.
 
 ## TODO
 - [ ] Finish Readme
-- [ ] Add CRC read
-- [ ] Check CRC
+- [x] Add CRC read
+- [x] Check CRC
 - [ ] DHT11 support
 - [ ] Test DHT11
 - [ ] Document code
\ No newline at end of file
diff --git a/readme_fr.md b/readme_fr.md
index 65413cc..0d008b8 100644
--- a/readme_fr.md
+++ b/readme_fr.md
@@ -29,8 +29,8 @@ Pour le moment le crates n'a été testé que sur un Raspberry Pico.
 
 ## TODO
 - [ ] Finir le Readme
-- [ ] Ajouter la lecture du CRC
-- [ ] Vérifier le CRC
+- [x] Ajouter la lecture du CRC
+- [x] Vérifier le CRC
 - [ ] Support du DHT11
 - [ ] Tester DHT11
 - [ ] Documenter le code
\ No newline at end of file
diff --git a/src/dht.pio b/src/dht.pio
index 98824f8..7d7c2d2 100644
--- a/src/dht.pio
+++ b/src/dht.pio
@@ -1,3 +1,4 @@
+.define bit_to_recv 32
 .program dht
     ; Clock must be set for 1µs per instruction
 
@@ -15,15 +16,20 @@
     wait 0 pin 0        ; Wait for low
     wait 1 pin 0        ; wait for high
 
-    set x, 31               ; Set number bit to received (ignore CRC :( sorry )
-    loop_data:          
-        wait 0 pin 0        ; Wait for low
-        wait 1 pin 0        ; wait for high
-        
-        nop [29]            ; wait for ~ 30 µS
+    set y, 4                ; set the number of byte to receive - 1
+    init_loop_data:
+        set x, 7               ; Set number bit to receive - 1
+        loop_data:          
+            wait 0 pin 0        ; Wait for low
+            wait 1 pin 0        ; wait for high
+            
+            nop [29]            ; wait for ~ 30 µS
 
-        in pins, 1          ; read the pin state and store bit value
-        
-        jmp x--, loop_data  ; Run until read all bits
-        
-    push                    ; send result
+            in pins, 1          ; read the pin state and store bit value
+            push iffull noblock ; Push data if full
+
+            jmp x--, loop_data  ; Run until read all bitsw
+
+    jmp y--, init_loop_data
+
+    push                    ; Push the last bytes
diff --git a/src/lib.rs b/src/lib.rs
index d5720aa..e136228 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -11,7 +11,7 @@ pub enum DhtError {
     /// Timeout during communication.
     Timeout,
     /// CRC mismatch.
-    CrcMismatch,
+    CrcMismatch(u32, u32),
     /// FIFO Read error
     ReadError,
 }
@@ -22,14 +22,14 @@ pub struct DhtResult {
     pub humidity: f32,
 }
 
-pub struct PicoDht<P: PIOExt, STI: StateMachineIndex> {
+pub struct DhtPio<P: PIOExt, STI: StateMachineIndex> {
     // sm: PicoStateMachine<P, STI>,
     sm: StateMachine<(P, STI), Running>,
     rx_fifo: Rx<(P, STI)>,
     tx_fifo: Tx<(P, STI)>,
 }
 
-impl<P: PIOExt, STI: StateMachineIndex> PicoDht<P, STI> {
+impl<P: PIOExt, STI: StateMachineIndex> DhtPio<P, STI> {
     pub fn new<I: AnyPin<Function = P::PinFunction>>(
         mut pio: rp2040_hal::pio::PIO<P>,
         sm: UninitStateMachine<(P, STI)>,
@@ -45,6 +45,7 @@ impl<P: PIOExt, STI: StateMachineIndex> PicoDht<P, STI> {
         let (mut sm, rx, tx) = rp2040_hal::pio::PIOBuilder::from_program(installed)
             .set_pins(pin.id().num, 1)
             .clock_divisor_fixed_point(int, frac)
+            .push_threshold(32)
             .build(sm);
         sm.set_pindirs([(pin.id().num, rp2040_hal::pio::PinDir::Output)]);
 
@@ -58,10 +59,22 @@ impl<P: PIOExt, STI: StateMachineIndex> PicoDht<P, STI> {
     /// Read data from the sensor. This blocking function (for maximum timeout of 2 seconds).
     pub fn read_data(&mut self, delay: &mut Delay) -> Result<DhtResult, DhtError> {
         let mut timeout = 2000;
+        let mut raw: [Option<u32>; 2] = [None; 2];
 
         self.tx_fifo.write(1);
 
-        while timeout > 0 && self.rx_fifo.is_empty() {
+        while timeout > 0 && raw[1].is_none() {
+            match self.rx_fifo.read() {
+                Some(d) => {
+                    if raw[0].is_none() {
+                        raw[0] = Some(d);
+                    } else {
+                        raw[1] = Some(d);
+                    }
+                }
+                None => (),
+            }
+
             delay.delay_ms(1);
             timeout -= 1;
         }
@@ -71,20 +84,32 @@ impl<P: PIOExt, STI: StateMachineIndex> PicoDht<P, STI> {
             return Err(DhtError::Timeout);
         }
 
-        let raw = match self.rx_fifo.read() {
-            Some(d) => d,
-            None => {
-                self.sm.restart();
-                return Err(DhtError::ReadError);
-            }
-        };
+        let data = raw[0].unwrap();
+        let crc = raw[1].unwrap();
 
-        let t_raw = raw & 0x0000FFFF;
-        let h_raw = (raw & 0xFFFF0000) >> 16;
+        if Self::compute_crc(data) != crc {
+            return Err(DhtError::CrcMismatch(
+                raw[0].unwrap_or(0),
+                raw[1].unwrap_or(0),
+            ));
+        }
+
+        let t_raw = data & 0x0000FFFF;
+        let h_raw = (data & 0xFFFF0000) >> 16;
 
         Ok(DhtResult {
             temperature: t_raw as f32 / 10.0,
             humidity: h_raw as f32 / 10.0,
         })
     }
+
+    fn compute_crc(data: u32) -> u32 {
+        let mut crc: u32 = 0;
+        crc += data & 0x000000FF;
+        crc += (data & 0x0000FF00) >> 8;
+        crc += (data & 0x00FF0000) >> 16;
+        crc += (data & 0xFF000000) >> 24;
+
+        crc % 256
+    }
 }