1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2002-2003 Takashi Sasai <sasai (at) sm.sony.co.jp> 6 * Copyright (C) 2003-2011 Marcel Holtmann <marcel (at) holtmann.org> 7 * 8 * 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License as published by 11 * the Free Software Foundation; either version 2 of the License, or 12 * (at your option) any later version. 13 * 14 * This program is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 * GNU General Public License for more details. 18 * 19 * You should have received a copy of the GNU General Public License 20 * along with this program; if not, write to the Free Software 21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 * 23 */ 24 25 #ifdef HAVE_CONFIG_H 26 #include <config.h> 27 #endif 28 29 #include <stdio.h> 30 #include <errno.h> 31 #include <unistd.h> 32 #include <stdlib.h> 33 #include <string.h> 34 #include <sys/types.h> 35 36 #include <net/ethertypes.h> 37 38 #include "parser.h" 39 40 /* BNEP Type */ 41 #define BNEP_GENERAL_ETHERNET 0x00 42 #define BNEP_CONTROL 0x01 43 #define BNEP_COMPRESSED_ETHERNET 0x02 44 #define BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY 0x03 45 #define BNEP_COMPRESSED_ETHERNET_DEST_ONLY 0x04 46 47 /* BNEP Control Packet Type */ 48 #define BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD 0x00 49 #define BNEP_SETUP_CONNECTION_REQUEST_MSG 0x01 50 #define BNEP_SETUP_CONNECTION_RESPONSE_MSG 0x02 51 #define BNEP_FILTER_NET_TYPE_SET_MSG 0x03 52 #define BNEP_FILTER_NET_TYPE_RESPONSE_MSG 0x04 53 #define BNEP_FILTER_MULT_ADDR_SET_MSG 0x05 54 #define BNEP_FILTER_MULT_ADDR_RESPONSE_MSG 0x06 55 56 /* BNEP Extension Type */ 57 #define BNEP_EXTENSION_CONTROL 0x00 58 59 #ifndef ETHERTYPE_IPV6 60 #define ETHERTYPE_IPV6 ETH_P_IPV6 61 #endif 62 63 static char *get_macaddr(struct frame *frm) 64 { 65 static char str[20]; 66 unsigned char *buf = frm->ptr; 67 68 sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x", 69 buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); 70 71 frm->ptr += 6; 72 frm->len -= 6; 73 74 return str; 75 } 76 77 static void bnep_control(int level, struct frame *frm, int header_length) 78 { 79 uint8_t uuid_size; 80 int i, length; 81 char *s; 82 uint32_t uuid = 0; 83 uint8_t type = get_u8(frm); 84 85 p_indent(++level, frm); 86 switch (type) { 87 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD: 88 printf("Not Understood(0x%02x) type 0x%02x\n", type, get_u8(frm)); 89 break; 90 91 case BNEP_SETUP_CONNECTION_REQUEST_MSG: 92 uuid_size = get_u8(frm); 93 printf("Setup Req(0x%02x) size 0x%02x ", type, uuid_size); 94 switch (uuid_size) { 95 case 2: 96 uuid = get_u16(frm); 97 printf("dst 0x%x", uuid); 98 if ((s = get_uuid_name(uuid)) != 0) 99 printf("(%s)", s); 100 uuid = get_u16(frm); 101 printf(" src 0x%x", uuid); 102 if ((s = get_uuid_name(uuid)) != 0) 103 printf("(%s)", s); 104 printf("\n"); 105 break; 106 case 4: 107 uuid = get_u32(frm); 108 printf("dst 0x%x", uuid); 109 if ((s = get_uuid_name(uuid)) != 0) 110 printf("(%s)", s); 111 uuid = get_u32(frm); 112 printf(" src 0x%x", uuid); 113 if ((s = get_uuid_name(uuid)) != 0) 114 printf("(%s)", s); 115 printf("\n"); 116 break; 117 case 16: 118 uuid = get_u32(frm); 119 printf("dst 0x%x", uuid); 120 if ((s = get_uuid_name(uuid)) != 0) 121 printf("(%s)", s); 122 frm->ptr += 12; 123 frm->len -= 12; 124 uuid = get_u32(frm); 125 printf(" src 0x%x", uuid); 126 if ((s = get_uuid_name(uuid)) != 0) 127 printf("(%s)", s); 128 printf("\n"); 129 frm->ptr += 12; 130 frm->len -= 12; 131 break; 132 default: 133 frm->ptr += (uuid_size * 2); 134 frm->len -= (uuid_size * 2); 135 break; 136 } 137 break; 138 139 case BNEP_SETUP_CONNECTION_RESPONSE_MSG: 140 printf("Setup Rsp(0x%02x) res 0x%04x\n", 141 type, get_u16(frm)); 142 break; 143 144 case BNEP_FILTER_NET_TYPE_SET_MSG: 145 length = get_u16(frm); 146 printf("Filter NetType Set(0x%02x) len 0x%04x\n", 147 type, length); 148 for (i = 0; i < length / 4; i++) { 149 p_indent(level + 1, frm); 150 printf("0x%04x - ", get_u16(frm)); 151 printf("0x%04x\n", get_u16(frm)); 152 } 153 break; 154 155 case BNEP_FILTER_NET_TYPE_RESPONSE_MSG: 156 printf("Filter NetType Rsp(0x%02x) res 0x%04x\n", 157 type, get_u16(frm)); 158 break; 159 160 case BNEP_FILTER_MULT_ADDR_SET_MSG: 161 length = get_u16(frm); 162 printf("Filter MultAddr Set(0x%02x) len 0x%04x\n", 163 type, length); 164 for (i = 0; i < length / 12; i++) { 165 p_indent(level + 1, frm); 166 printf("%s - ", get_macaddr(frm)); 167 printf("%s\n", get_macaddr(frm)); 168 } 169 break; 170 171 case BNEP_FILTER_MULT_ADDR_RESPONSE_MSG: 172 printf("Filter MultAddr Rsp(0x%02x) res 0x%04x\n", 173 type, get_u16(frm)); 174 break; 175 176 default: 177 printf("Unknown control type(0x%02x)\n", type); 178 raw_ndump(level + 1, frm, header_length - 1); 179 frm->ptr += header_length - 1; 180 frm->len -= header_length - 1; 181 return; 182 } 183 } 184 185 static void bnep_eval_extension(int level, struct frame *frm) 186 { 187 uint8_t type = get_u8(frm); 188 uint8_t length = get_u8(frm); 189 int extension = type & 0x80; 190 191 p_indent(level, frm); 192 193 switch (type & 0x7f) { 194 case BNEP_EXTENSION_CONTROL: 195 printf("Ext Control(0x%02x|%s) len 0x%02x\n", 196 type & 0x7f, extension ? "1" : "0", length); 197 bnep_control(level, frm, length); 198 break; 199 200 default: 201 printf("Ext Unknown(0x%02x|%s) len 0x%02x\n", 202 type & 0x7f, extension ? "1" : "0", length); 203 raw_ndump(level + 1, frm, length); 204 frm->ptr += length; 205 frm->len -= length; 206 } 207 208 if (extension) 209 bnep_eval_extension(level, frm); 210 } 211 212 void bnep_dump(int level, struct frame *frm) 213 { 214 uint8_t type = get_u8(frm); 215 uint16_t proto = 0x0000; 216 int extension = type & 0x80; 217 218 p_indent(level, frm); 219 220 switch (type & 0x7f) { 221 case BNEP_CONTROL: 222 printf("BNEP: Control(0x%02x|%s)\n", 223 type & 0x7f, extension ? "1" : "0"); 224 bnep_control(level, frm, -1); 225 break; 226 227 case BNEP_COMPRESSED_ETHERNET: 228 printf("BNEP: Compressed(0x%02x|%s)\n", 229 type & 0x7f, extension ? "1" : "0"); 230 p_indent(++level, frm); 231 proto = get_u16(frm); 232 printf("[proto 0x%04x]\n", proto); 233 break; 234 235 case BNEP_GENERAL_ETHERNET: 236 printf("BNEP: General ethernet(0x%02x|%s)\n", 237 type & 0x7f, extension ? "1" : "0"); 238 p_indent(++level, frm); 239 printf("dst %s ", get_macaddr(frm)); 240 printf("src %s ", get_macaddr(frm)); 241 proto = get_u16(frm); 242 printf("[proto 0x%04x]\n", proto); 243 break; 244 245 case BNEP_COMPRESSED_ETHERNET_DEST_ONLY: 246 printf("BNEP: Compressed DestOnly(0x%02x|%s)\n", 247 type & 0x7f, extension ? "1" : "0"); 248 p_indent(++level, frm); 249 printf("dst %s ", get_macaddr(frm)); 250 proto = get_u16(frm); 251 printf("[proto 0x%04x]\n", proto); 252 break; 253 254 case BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY: 255 printf("BNEP: Compressed SrcOnly(0x%02x|%s)\n", 256 type & 0x7f, extension ? "1" : "0"); 257 p_indent(++level, frm); 258 printf("src %s ", get_macaddr(frm)); 259 proto = get_u16(frm); 260 printf("[proto 0x%04x]\n", proto); 261 break; 262 263 default: 264 printf("(Unknown packet type)\n"); 265 return; 266 } 267 268 /* Extension info */ 269 if (extension) 270 bnep_eval_extension(++level, frm); 271 272 /* Control packet => No payload info */ 273 if ((type & 0x7f) == BNEP_CONTROL) 274 return; 275 276 /* 802.1p header */ 277 if (proto == 0x8100) { 278 p_indent(level, frm); 279 printf("802.1p Header: 0x%04x ", get_u16(frm)); 280 proto = get_u16(frm); 281 printf("[proto 0x%04x]\n", proto); 282 } 283 284 if (!(parser.flags & DUMP_VERBOSE)) { 285 raw_dump(level, frm); 286 return; 287 } 288 289 switch (proto) { 290 case ETHERTYPE_ARP: 291 p_indent(++level, frm); 292 printf("ARP: "); 293 arp_dump(level, frm); 294 break; 295 296 case ETHERTYPE_REVARP: 297 p_indent(++level, frm); 298 printf("RARP: "); 299 arp_dump(level, frm); 300 break; 301 302 case ETHERTYPE_IP: 303 p_indent(++level, frm); 304 printf("IP: "); 305 ip_dump(level, frm); 306 break; 307 308 case ETHERTYPE_IPV6: 309 p_indent(++level, frm); 310 printf("IPV6: "); 311 ip_dump(level, frm); 312 break; 313 314 default: 315 raw_dump(level, frm); 316 break; 317 } 318 } 319