From 871d80ffa3eed46d249c1f7a291604736eb273d3 Mon Sep 17 00:00:00 2001
From: gpotter2 <gabriel@potter.fr>
Date: Fri, 27 Oct 2017 04:11:15 +0200
Subject: [PATCH] Fix HCI_ACL_Hdr fields parsing

---
 scapy/layers/bluetooth.py | 28 ++++++++++++++++++++++++----
 test/bluetooth.uts        |  6 ++++++
 2 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/scapy/layers/bluetooth.py b/scapy/layers/bluetooth.py
index 4f63e0f21..577fa6f60 100644
--- a/scapy/layers/bluetooth.py
+++ b/scapy/layers/bluetooth.py
@@ -79,14 +79,34 @@ class HCI_Hdr(Packet):
 
 class HCI_ACL_Hdr(Packet):
     name = "HCI ACL header"
-    fields_desc = [ ByteField("handle",0), # Actually, handle is 12 bits and flags is 4.
-                    ByteField("flags",0),  # I wait to write a LEBitField
+    fields_desc = [ BitField("handle",0,12),    # TODO: Create and use LEBitField
+                    BitField("PB",0,2),      # They are recieved as a **combined** LE Short
+                    BitField("BC",0,2),      # Handle is 12 bits, eacg flag is 2 bits.
                     LEShortField("len",None), ]
+
+    def pre_dissect(self, s):
+        # Recieve data as LE stored as
+        # .... 1111 0100 1100 = handle
+        # 1010 .... .... .... = flags
+        # And turn it into
+        # 1111 0100 1100 .... = handle
+        # .... .... .... 1010 = flags
+        hf = socket.ntohs(struct.unpack("!H", s[:2])[0])
+        r = ((hf & 0x0fff) << 4) + (hf >> 12)
+        return struct.pack("!H", r) + s[2:]
+
+    def post_dissect(self, s):
+        self.raw_packet_cache = None # Reset packet to allow post_build
+        return s
+
     def post_build(self, p, pay):
         p += pay
         if self.len is None:
             p = p[:2] + struct.pack("<H", len(pay)) + p[4:]
-        return p
+        # Reverse, opposite of pre_dissect
+        hf = struct.unpack("!H", p[:2])[0]
+        r = socket.ntohs(((hf & 0xf) << 12) + (hf >> 4))
+        return struct.pack("!H", r) + p[2:]
 
 
 class L2CAP_Hdr(Packet):
@@ -689,7 +709,7 @@ bind_layers( HCI_Hdr,       HCI_ACL_Hdr,        type=2)
 bind_layers( HCI_Hdr,       HCI_Event_Hdr,      type=4)
 bind_layers( HCI_Hdr,       conf.raw_layer,           )
 
-conf.l2types.register_num2layer(LINKTYPE_BLUETOOTH_HCI_H4, HCI_Hdr)
+conf.l2types.register(LINKTYPE_BLUETOOTH_HCI_H4, HCI_Hdr)
 
 bind_layers( HCI_Command_Hdr, HCI_Cmd_Reset, opcode=0x0c03)
 bind_layers( HCI_Command_Hdr, HCI_Cmd_Set_Event_Mask, opcode=0x0c01)
diff --git a/test/bluetooth.uts b/test/bluetooth.uts
index 2be2de60f..6f1ddc2d3 100644
--- a/test/bluetooth.uts
+++ b/test/bluetooth.uts
@@ -60,6 +60,12 @@ assert a[SM_Identity_Address_Information].address == 'a1:b2:c3:d4:e5:f6'
 assert a[SM_Identity_Address_Information].atype == 0
 a.show()
 
+= Basic HCI_ACL_Hdr build & dissect
+a = HCI_Hdr()/HCI_ACL_Hdr(handle=0xf4c, PB=2, BC=2, len=20)/L2CAP_Hdr(len=16)/L2CAP_CmdHdr(code=8, len=12)/Raw("A"*12)
+assert raw(a) == b'\x02L\xaf\x14\x00\x10\x00\x05\x00\x08\x00\x0c\x00AAAAAAAAAAAA'
+b = HCI_Hdr(raw(a))
+assert a == b
+
 = Complex HCI - L2CAP build
 a = HCI_Hdr()/HCI_ACL_Hdr()/L2CAP_Hdr()/L2CAP_CmdHdr()/L2CAP_ConnReq(scid=1)
 assert raw(a) == b'\x02\x00\x00\x0c\x00\x08\x00\x05\x00\x02\x00\x04\x00\x00\x00\x01\x00'