1 // 2 // Copyright (c) 2010-2023 Antmicro 3 // 4 // This file is licensed under the MIT License. 5 // Full license text is available in 'licenses/MIT.txt'. 6 // 7 using System; 8 using System.Collections.Generic; 9 using Antmicro.Renode.Peripherals.Wireless; 10 using Antmicro.Renode.Logging; 11 using Antmicro.Renode.Utilities; 12 13 namespace Antmicro.Renode.Plugins.WiresharkPlugin 14 { 15 public class BLESniffer 16 { InsertHeaderToPacket(IRadio sender, byte[] originalPacket)17 public byte[] InsertHeaderToPacket(IRadio sender, byte[] originalPacket) 18 { 19 byte[] completePacket = new byte[originalPacket.Length + SnifferMetadataSizeInBytes]; 20 21 // If packet is shorter than minimal acceptable or channel is out of range then communication is broken 22 // and we can abort sniffing anything. 23 if(originalPacket.Length < MinimalBLEPacketLength) 24 { 25 sender.Log(LogLevel.Error, "BLE packet size is {0} bytes. Expected at least {1} bytes.", originalPacket.Length, MinimalBLEPacketLength); 26 FillRestOfData(originalPacket, completePacket, SnifferMetadataSizeInBytes); 27 return completePacket; 28 } 29 30 if(!channelToWiresharkIndex.TryGetValue(sender.Channel, out var wiresharkIndex)) 31 { 32 sender.Log(LogLevel.Error, "Channel number {0} doesn't exist in bluetooth specification.", sender.Channel); 33 FillRestOfData(originalPacket, completePacket, SnifferMetadataSizeInBytes); 34 return completePacket; 35 } 36 37 completePacket[0] = wiresharkIndex; 38 39 // Signal Power 40 completePacket[1] = 0x0; 41 42 // Noise Power 43 completePacket[2] = 0x0; 44 45 // Access Address Offenses 46 completePacket[3] = 0x0; 47 48 // Reference Access Address 49 completePacket[4] = originalPacket[0]; 50 completePacket[5] = originalPacket[1]; 51 completePacket[6] = originalPacket[2]; 52 completePacket[7] = originalPacket[3]; 53 54 var flags = (short)SnifferFlags.PacketDeWhitened | (short)SnifferFlags.SignalPowerIsValid | (short)SnifferFlags.NoisePowerIsValid | (short)SnifferFlags.PacketIsDecrypted 55 | (short)SnifferFlags.ReferenceAccessAddressIsValid | (short)SnifferFlags.AccessAddressOffensesIsValid | (short)SnifferFlags.CRCWasChecked 56 | (short)SnifferFlags.CRCIsValid | (short)SnifferFlags.MICWasChecked | (short)SnifferFlags.MICIsValid; 57 58 var accessAddress = BitHelper.ToUInt32(originalPacket, 0, 4, true); 59 60 // Here we check whether we have an advertisement packet or data packet. We're using access address embedded in packet. 61 if(accessAddress != AdvertisementAccessAddress) 62 { 63 // If an accessAddress is a key in the dictionary then we know that sender belongs to this connection. 64 // There is an assumption that data packets are sent after advertisement packets. If there is no key that matches access address then something went wrong. 65 // If sender is a value for this key then it's a master, otherwise it's a slave. 66 if(!mastersInConnections.TryGetValue(accessAddress, out var masterRadio)) 67 { 68 sender.Log(LogLevel.Error, "There is no connection associated with accessAddress: 0x{0:X}.", accessAddress); 69 FillRestOfData(originalPacket, completePacket, SnifferMetadataSizeInBytes); 70 return completePacket; 71 } 72 73 if(masterRadio == sender) 74 { 75 flags |= (short)PDUTypeNumbers.DataPacketMasterToSlave; 76 } 77 else 78 { 79 flags |= (short)PDUTypeNumbers.DataPacketSlaveToMaster; 80 } 81 } 82 else 83 { 84 flags |= (short)PDUTypeNumbers.Advertisement; 85 86 // When CONNECT_IND packet (PDU type 5) is sent we can read from PDU payload what access address will be used for this connection. 87 // This way we can save connection's access address and which radio is a master for this connection. 88 var pduType = originalPacket[4] & 0xF; 89 if(pduType == ConnectPDUType) 90 { 91 if(originalPacket.Length < ConnectBLEPacketLength) 92 { 93 sender.Log(LogLevel.Error, "Connect packet size is {0} bytes. Expected at least {1} bytes.", originalPacket.Length, ConnectBLEPacketLength); 94 FillRestOfData(originalPacket, completePacket, SnifferMetadataSizeInBytes); 95 return completePacket; 96 } 97 98 // Extract an accessAddress for connection from PDU payload. 99 var newAccessAddress = BitHelper.ToUInt32(originalPacket, 18, 4, true); 100 if(mastersInConnections.ContainsKey(newAccessAddress)) 101 { 102 // connection with this access address already exists, so we override it 103 sender.Log(LogLevel.Warning, "There is already a connection associated with access address: 0x{0:X}. Overriding old connection.", newAccessAddress); 104 mastersInConnections[newAccessAddress] = sender; 105 } 106 else 107 { 108 mastersInConnections.Add(newAccessAddress, sender); 109 } 110 } 111 112 // Sniffer just sets an appropriate flag in header to indicate that it's auxiliary advertisement packet. 113 else if(pduType == AuxiliaryAdvertisementPDUType) 114 { 115 flags |= (short)PDUTypeNumbers.AuxiliaryAdvertisement; 116 } 117 } 118 119 // Embed flags to the packet. 120 completePacket[8] = (byte)flags; 121 completePacket[9] = (byte)(flags >> 8); 122 123 FillRestOfData(originalPacket, completePacket, SnifferMetadataSizeInBytes); 124 return completePacket; 125 } 126 FillRestOfData(byte[] src, byte[] dest, int offset)127 private void FillRestOfData(byte[] src, byte[] dest, int offset) 128 { 129 for(int i = 0; i < src.Length; i++) 130 { 131 dest[i + offset] = src[i]; 132 } 133 } 134 135 private const int SnifferMetadataSizeInBytes = 10; 136 private const int MinimalBLEPacketLength = 9; 137 private const int ConnectBLEPacketLength = 43; 138 private const uint AdvertisementAccessAddress = 0x8e89bed6; 139 private const byte ConnectPDUType = 5; 140 private const byte AuxiliaryAdvertisementPDUType = 7; 141 142 // Wireshark indexes channels from 0 to 39 basing on the frequency meanwhile in reality channels 37, 38 and 39 143 // are in different places of frequency spectrum. This is why we have to remap channel numbers to indexed for Wireshark. 144 private readonly Dictionary<int, byte> channelToWiresharkIndex = new Dictionary<int, byte>() 145 { 146 { 37, 0 }, 147 { 0, 1 }, 148 { 1, 2 }, 149 { 2, 3 }, 150 { 3, 4 }, 151 { 4, 5 }, 152 { 5, 6 }, 153 { 6, 7 }, 154 { 7, 8 }, 155 { 8, 9 }, 156 { 9, 10 }, 157 { 10, 11 }, 158 { 38, 12 }, 159 { 11, 13 }, 160 { 12, 14 }, 161 { 13, 15 }, 162 { 14, 16 }, 163 { 15, 17 }, 164 { 16, 18 }, 165 { 17, 19 }, 166 { 18, 20 }, 167 { 19, 21 }, 168 { 20, 22 }, 169 { 21, 23 }, 170 { 22, 24 }, 171 { 23, 25 }, 172 { 24, 26 }, 173 { 25, 27 }, 174 { 26, 28 }, 175 { 27, 29 }, 176 { 28, 30 }, 177 { 29, 31 }, 178 { 30, 32 }, 179 { 31, 33 }, 180 { 32, 34 }, 181 { 33, 35 }, 182 { 34, 36 }, 183 { 35, 37 }, 184 { 36, 38 }, 185 { 39, 39 }, 186 }; 187 188 // This dictionary holds an information about all connections. Each connection is identified by an access address 189 // and there is only one master device associated with each access address. Other devices associated with access addressses 190 // are considered to be slaves. 191 private readonly Dictionary<uint, IRadio> mastersInConnections = new Dictionary<uint, IRadio>(); 192 193 // Flags registers are made of a bitfield and pdu type numbers 194 [Flags] 195 private enum SnifferFlags : short 196 { 197 None = 0x0, 198 PacketDeWhitened = 0x1, 199 SignalPowerIsValid = 0x2, 200 NoisePowerIsValid = 0x4, 201 PacketIsDecrypted = 0x8, 202 ReferenceAccessAddressIsValid = 0x10, 203 AccessAddressOffensesIsValid = 0x20, 204 RFChannelIsLikelyToBeDistorted = 0x40, 205 CRCWasChecked = 0x400, 206 CRCIsValid = 0x800, 207 MICWasChecked = 0x1000, 208 MICIsValid = 0x2000, 209 } 210 211 private enum PDUTypeNumbers : short 212 { 213 Advertisement = 0x0 << 7, 214 AuxiliaryAdvertisement = 0x1 << 7, 215 DataPacketMasterToSlave = 0x2 << 7, 216 DataPacketSlaveToMaster = 0x3 << 7 217 } 218 } 219 } 220
