tools/parser/bnep.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2002-2003  Takashi Sasai <sasai@sm.sony.co.jp>
   6  *  Copyright (C) 2003-2011  Marcel Holtmann <marcel@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
  35 #include <net/ethernet.h>
  36
  37 #include "parser.h"
  38
  39 /* BNEP Type */
  40 #define BNEP_GENERAL_ETHERNET                   0x00
  41 #define BNEP_CONTROL                            0x01
  42 #define BNEP_COMPRESSED_ETHERNET                0x02
  43 #define BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY    0x03
  44 #define BNEP_COMPRESSED_ETHERNET_DEST_ONLY      0x04
  45
  46 /* BNEP Control Packet Type */
  47 #define BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD     0x00
  48 #define BNEP_SETUP_CONNECTION_REQUEST_MSG       0x01
  49 #define BNEP_SETUP_CONNECTION_RESPONSE_MSG      0x02
  50 #define BNEP_FILTER_NET_TYPE_SET_MSG            0x03
  51 #define BNEP_FILTER_NET_TYPE_RESPONSE_MSG       0x04
  52 #define BNEP_FILTER_MULT_ADDR_SET_MSG           0x05
  53 #define BNEP_FILTER_MULT_ADDR_RESPONSE_MSG      0x06
  54
  55 /* BNEP Extension Type */
  56 #define BNEP_EXTENSION_CONTROL                  0x00
  57
  58 #ifndef ETHERTYPE_IPV6
  59 #define ETHERTYPE_IPV6 ETH_P_IPV6
  60 #endif
  61
  62 static char *get_macaddr(struct frame *frm)
  63 {
  64         static char str[20];
  65         unsigned char *buf = frm->ptr;
  66
  67         sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x",
  68                 buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
  69
  70         frm->ptr += 6;
  71         frm->len -= 6;
  72
  73         return str;
  74 }
  75
  76 static void bnep_control(int level, struct frame *frm, int header_length)
  77 {
  78         uint8_t uuid_size;
  79         int i, length;
  80         char *s;
  81         uint32_t uuid = 0;
  82         uint8_t type = p_get_u8(frm);
  83
  84         p_indent(++level, frm);
  85         switch (type) {
  86         case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
  87                 printf("Not Understood(0x%02x) type 0x%02x\n", type, p_get_u8(frm));
  88                 break;
  89
  90         case BNEP_SETUP_CONNECTION_REQUEST_MSG:
  91                 uuid_size = p_get_u8(frm);
  92                 printf("Setup Req(0x%02x) size 0x%02x ", type, uuid_size);
  93                 switch (uuid_size) {
  94                 case 2:
  95                         uuid = p_get_u16(frm);
  96                         printf("dst 0x%x", uuid);
  97                         if ((s = get_uuid_name(uuid)) != 0)
  98                                 printf("(%s)", s);
  99                         uuid = p_get_u16(frm);
 100                         printf(" src 0x%x", uuid);
 101                         if ((s = get_uuid_name(uuid)) != 0)
 102                                 printf("(%s)", s);
 103                         printf("\n");
 104                         break;
 105                 case 4:
 106                         uuid = p_get_u32(frm);
 107                         printf("dst 0x%x", uuid);
 108                         if ((s = get_uuid_name(uuid)) != 0)
 109                                 printf("(%s)", s);
 110                         uuid = p_get_u32(frm);
 111                         printf(" src 0x%x", uuid);
 112                         if ((s = get_uuid_name(uuid)) != 0)
 113                                 printf("(%s)", s);
 114                         printf("\n");
 115                         break;
 116                 case 16:
 117                         uuid = p_get_u32(frm);
 118                         printf("dst 0x%x", uuid);
 119                         if ((s = get_uuid_name(uuid)) != 0)
 120                                 printf("(%s)", s);
 121                         frm->ptr += 12;
 122                         frm->len -= 12;
 123                         uuid = p_get_u32(frm);
 124                         printf(" src 0x%x", uuid);
 125                         if ((s = get_uuid_name(uuid)) != 0)
 126                                 printf("(%s)", s);
 127                         printf("\n");
 128                         frm->ptr += 12;
 129                         frm->len -= 12;
 130                         break;
 131                 default:
 132                         frm->ptr += (uuid_size * 2);
 133                         frm->len -= (uuid_size * 2);
 134                         break;
 135                 }
 136                 break;
 137
 138         case BNEP_SETUP_CONNECTION_RESPONSE_MSG:
 139                 printf("Setup Rsp(0x%02x) res 0x%04x\n",
 140                                                 type, p_get_u16(frm));
 141                 break;
 142
 143         case BNEP_FILTER_NET_TYPE_SET_MSG:
 144                 length = p_get_u16(frm);
 145                 printf("Filter NetType Set(0x%02x) len 0x%04x\n",
 146                                                         type, length);
 147                 for (i = 0; i < length / 4; i++) {
 148                         p_indent(level + 1, frm);
 149                         printf("0x%04x - ", p_get_u16(frm));
 150                         printf("0x%04x\n", p_get_u16(frm));
 151                 }
 152                 break;
 153
 154         case BNEP_FILTER_NET_TYPE_RESPONSE_MSG:
 155                 printf("Filter NetType Rsp(0x%02x) res 0x%04x\n",
 156                                                         type, p_get_u16(frm));
 157                 break;
 158
 159         case BNEP_FILTER_MULT_ADDR_SET_MSG:
 160                 length = p_get_u16(frm);
 161                 printf("Filter MultAddr Set(0x%02x) len 0x%04x\n",
 162                                                         type, length);
 163                 for (i = 0; i < length / 12; i++) {
 164                         p_indent(level + 1, frm);
 165                         printf("%s - ", get_macaddr(frm));
 166                         printf("%s\n", get_macaddr(frm));
 167                 }
 168                 break;
 169
 170         case BNEP_FILTER_MULT_ADDR_RESPONSE_MSG:
 171                 printf("Filter MultAddr Rsp(0x%02x) res 0x%04x\n",
 172                                                         type, p_get_u16(frm));
 173                 break;
 174
 175         default:
 176                 printf("Unknown control type(0x%02x)\n", type);
 177                 raw_ndump(level + 1, frm, header_length - 1);
 178                 frm->ptr += header_length - 1;
 179                 frm->len -= header_length - 1;
 180                 return;
 181         }
 182 }
 183
 184 static void bnep_eval_extension(int level, struct frame *frm)
 185 {
 186         uint8_t type = p_get_u8(frm);
 187         uint8_t length = p_get_u8(frm);
 188         int extension = type & 0x80;
 189
 190         p_indent(level, frm);
 191
 192         switch (type & 0x7f) {
 193         case BNEP_EXTENSION_CONTROL:
 194                 printf("Ext Control(0x%02x|%s) len 0x%02x\n",
 195                                 type & 0x7f, extension ? "1" : "0", length);
 196                 bnep_control(level, frm, length);
 197                 break;
 198
 199         default:
 200                 printf("Ext Unknown(0x%02x|%s) len 0x%02x\n",
 201                                 type & 0x7f, extension ? "1" : "0", length);
 202                 raw_ndump(level + 1, frm, length);
 203                 frm->ptr += length;
 204                 frm->len -= length;
 205         }
 206
 207         if (extension)
 208                 bnep_eval_extension(level, frm);
 209 }
 210
 211 void bnep_dump(int level, struct frame *frm)
 212 {
 213         uint8_t type = p_get_u8(frm);
 214         uint16_t proto = 0x0000;
 215         int extension = type & 0x80;
 216
 217         p_indent(level, frm);
 218
 219         switch (type & 0x7f) {
 220         case BNEP_CONTROL:
 221                 printf("BNEP: Control(0x%02x|%s)\n",
 222                                         type & 0x7f, extension ? "1" : "0");
 223                 bnep_control(level, frm, -1);
 224                 break;
 225
 226         case BNEP_COMPRESSED_ETHERNET:
 227                 printf("BNEP: Compressed(0x%02x|%s)\n",
 228                                         type & 0x7f, extension ? "1" : "0");
 229                 p_indent(++level, frm);
 230                 proto = p_get_u16(frm);
 231                 printf("[proto 0x%04x]\n", proto);
 232                 break;
 233
 234         case BNEP_GENERAL_ETHERNET:
 235                 printf("BNEP: General ethernet(0x%02x|%s)\n",
 236                                         type & 0x7f, extension ? "1" : "0");
 237                 p_indent(++level, frm);
 238                 printf("dst %s ", get_macaddr(frm));
 239                 printf("src %s ", get_macaddr(frm));
 240                 proto = p_get_u16(frm);
 241                 printf("[proto 0x%04x]\n", proto);
 242                 break;
 243
 244         case BNEP_COMPRESSED_ETHERNET_DEST_ONLY:
 245                 printf("BNEP: Compressed DestOnly(0x%02x|%s)\n",
 246                                         type & 0x7f, extension ? "1" : "0");
 247                 p_indent(++level, frm);
 248                 printf("dst %s ", get_macaddr(frm));
 249                 proto = p_get_u16(frm);
 250                 printf("[proto 0x%04x]\n", proto);
 251                 break;
 252
 253         case BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY:
 254                 printf("BNEP: Compressed SrcOnly(0x%02x|%s)\n",
 255                                         type & 0x7f, extension ? "1" : "0");
 256                 p_indent(++level, frm);
 257                 printf("src %s ", get_macaddr(frm));
 258                 proto = p_get_u16(frm);
 259                 printf("[proto 0x%04x]\n", proto);
 260                 break;
 261
 262         default:
 263                 printf("(Unknown packet type)\n");
 264                 return;
 265         }
 266
 267         /* Extension info */
 268         if (extension)
 269                 bnep_eval_extension(++level, frm);
 270
 271         /* Control packet => No payload info */
 272         if ((type & 0x7f) == BNEP_CONTROL)
 273                 return;
 274
 275         /* 802.1p header */
 276         if (proto == 0x8100) {
 277                 p_indent(level, frm);
 278                 printf("802.1p Header: 0x%04x ", p_get_u16(frm));
 279                 proto = p_get_u16(frm);
 280                 printf("[proto 0x%04x]\n", proto);
 281         }
 282
 283         if (!(parser.flags & DUMP_VERBOSE)) {
 284                 raw_dump(level, frm);
 285                 return;
 286         }
 287
 288         switch (proto) {
 289         case ETHERTYPE_ARP:
 290                 p_indent(++level, frm);
 291                 printf("ARP: ");
 292                 arp_dump(level, frm);
 293                 break;
 294
 295         case ETHERTYPE_REVARP:
 296                 p_indent(++level, frm);
 297                 printf("RARP: ");
 298                 arp_dump(level, frm);
 299                 break;
 300
 301         case ETHERTYPE_IP:
 302                 p_indent(++level, frm);
 303                 printf("IP: ");
 304                 ip_dump(level, frm);
 305                 break;
 306
 307         case ETHERTYPE_IPV6:
 308                 p_indent(++level, frm);
 309                 printf("IPV6: ");
 310                 ip_dump(level, frm);
 311                 break;
 312
 313         default:
 314                 raw_dump(level, frm);
 315                 break;
 316         }
 317 }