diff --git a/src/tea3.rs b/src/tea3.rs
index c8fdff3..e90b7ff 100644
--- a/src/tea3.rs
+++ b/src/tea3.rs
@@ -44,6 +44,28 @@ impl Tea3 {
         kout
     }
 
+    pub fn step_state_register(&mut self, kout: u8) -> u8 {
+        let r7 = self.state_register[7];
+
+        let bp_r4 = bp(self.state_register[4]);
+        let f2_r2_r1 = f2(e(self.state_register[2], self.state_register[1]));
+        let f1_r6_r5 = f1(e(self.state_register[6], self.state_register[5]));
+
+        let r0_new = r7 ^ bp_r4 ^ f2_r2_r1 ^ kout;
+        let r5_new = self.state_register[4] ^ f1_r6_r5;
+
+        self.state_register[7] = self.state_register[6];
+        self.state_register[6] = self.state_register[5];
+        self.state_register[5] = r5_new;
+        self.state_register[4] = self.state_register[3];
+        self.state_register[3] = self.state_register[2];
+        self.state_register[2] = self.state_register[1];
+        self.state_register[1] = self.state_register[0];
+        self.state_register[0] = r0_new;
+
+        r7
+    }
+
     pub fn key_register(&self) -> &[u8] {
         &self.key_register
     }
@@ -74,6 +96,95 @@ const TEA3_P: [u8; 256] = [
     0x52, 0x8C, 0x5D, 0x29, 0x6D, 0x04, 0xBC, 0x25, 0x15, 0x8B, 0x12, 0x9B, 0xD6, 0x75, 0xA3, 0x97,
 ];
 
+/// ETSI TS 104 053-1 Figure 17
+const T_F1: [[u8; 16]; 8] = [
+    [1, 1, 0, 0, 1, 0, 0, 1, 0, 1, 1, 1, 0, 1, 0, 0], //S1
+    [1, 1, 0, 0, 1, 0, 0, 1, 1, 0, 1, 1, 0, 1, 0, 0], //S2
+    [1, 0, 0, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 1], //S3
+    [1, 1, 0, 1, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1], //S4
+    [0, 0, 1, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 0], //S5
+    [0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 0, 0], //S6
+    [1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 0, 0, 1, 0, 1], //S7
+    [0, 0, 0, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 0, 1], //S8
+];
+
+/// ETSI TS 104 053-1 Figure 18
+const T_F2: [[u8; 16]; 8] = [
+    [1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 1, 1, 1, 0], //S1
+    [0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 0], //S2
+    [0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 0, 0, 0, 0], //S3
+    [0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 0, 0, 0], //S4
+    [0, 0, 1, 1, 0, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 0], //S5
+    [0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 0, 1], //S6
+    [1, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 0, 1, 0, 1], //S7
+    [1, 1, 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 1], //S8
+];
+
+fn f1(n: [u8; 8]) -> u8 {
+    truth_table_byte(&T_F1, n)
+}
+
+fn f2(n: [u8; 8]) -> u8 {
+    truth_table_byte(&T_F2, n)
+}
+
+fn truth_table_byte(table: &[[u8; 16]; 8], nibbles: [u8; 8]) -> u8 {
+    let mut out = 0u8;
+
+    for (i, row) in table.iter().enumerate() {
+        let idx = nibbles[i] as usize;
+        assert!(idx < 16, "nibble out of range");
+        out |= (row[idx] & 1) << (7 - i);
+    }
+
+    out
+}
+
+fn bit(b: u8, pos: u8) -> u8 {
+    (b >> (8 - pos)) & 1
+}
+
+fn e(byte1: u8, byte2: u8) -> [u8; 8] {
+    let get = |pos: u8| -> u8 {
+        if pos <= 8 {
+            bit(byte1, pos)
+        } else {
+            bit(byte2, pos - 8)
+        }
+    };
+
+    let nibble = |a: u8, b: u8, c: u8, d: u8| -> u8 {
+        (get(a) << 3) | (get(b) << 2) | (get(c) << 1) | get(d)
+    };
+
+    [
+        nibble(3, 4, 11, 12),
+        nibble(4, 5, 12, 13),
+        nibble(5, 6, 13, 14),
+        nibble(6, 7, 14, 15),
+        nibble(7, 8, 15, 16),
+        nibble(8, 1, 16, 9),
+        nibble(1, 2, 9, 10),
+        nibble(2, 3, 10, 11),
+    ]
+}
+
+/// BP.
+/// input bits numbered: 12345678 (bit 1 MSB),
+/// output: 38467215.
+fn bp(x: u8) -> u8 {
+    let bit = |pos: u8| -> u8 { (x >> (8 - pos)) & 1 };
+
+    (bit(3) << 7)
+        | (bit(8) << 6)
+        | (bit(4) << 5)
+        | (bit(6) << 4)
+        | (bit(7) << 3)
+        | (bit(2) << 2)
+        | (bit(1) << 1)
+        | bit(5)
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;
@@ -135,4 +246,76 @@ mod tests {
             "g + h LFSRs must reproduce the exact output sequence"
         );
     }
+
+    #[test]
+    fn test_bp() {
+        let x = 0b10110010;
+        // bits:
+        // 1:1 2:0 3:1 4:1 5:0 6:0 7:1 8:0
+
+        // BP mapping: [3,8,4,6,7,2,1,5] -> [1,0,1,0,1,0,1,0] = 0b10101010
+
+        let result = bp(x);
+        assert_eq!(result, 0b10101010);
+    }
+
+    #[test]
+    fn test_f1() {
+        // Input: 8 nibbles (0..15)
+        let input = [0, 1, 2, 3, 4, 5, 6, 7];
+
+        let out = f1(input);
+
+        // Expected: lookup from T_F1
+        let expected = (T_F1[0][0] << 7)
+            | (T_F1[1][1] << 6)
+            | (T_F1[2][2] << 5)
+            | (T_F1[3][3] << 4)
+            | (T_F1[4][4] << 3)
+            | (T_F1[5][5] << 2)
+            | (T_F1[6][6] << 1)
+            | T_F1[7][7];
+
+        assert_eq!(out, expected);
+    }
+
+    #[test]
+    fn test_f2() {
+        let input = [15, 14, 13, 12, 11, 10, 9, 8];
+
+        let out = f2(input);
+
+        let expected = (T_F2[0][15] << 7)
+            | (T_F2[1][14] << 6)
+            | (T_F2[2][13] << 5)
+            | (T_F2[3][12] << 4)
+            | (T_F2[4][11] << 3)
+            | (T_F2[5][10] << 2)
+            | (T_F2[6][9] << 1)
+            | T_F2[7][8];
+
+        assert_eq!(out, expected);
+    }
+
+    #[test]
+    fn test_e() {
+        let b1 = 0b11110000;
+        let b2 = 0b00001111;
+
+        let out = e(b1, b2);
+
+        assert_eq!(out.len(), 8);
+
+        // S1 = (3,4,11,12)
+        // b1 bits: 1 1 1 1 0 0 0 0
+        // b2 bits: 0 0 0 0 1 1 1 1
+        //
+        // 3=1, 4=1, 11=0, 12=0 -> 1100 = 0xC
+
+        assert_eq!(out[0], 0b1100);
+
+        // S5 = (7,8,15,16)
+        // 7=0, 8=0, 15=1, 16=1 -> 0011 = 0x3
+        assert_eq!(out[4], 0b0011);
+    }
 }