senf/Packets/80211Bundle/RadiotapPacket.cc

   1 // $Id$
   2 //
   3 // Copyright (C) 2008
   4 // Fraunhofer Institute for Open Communication Systems (FOKUS)
   5 // Competence Center NETwork research (NET), St. Augustin, GERMANY
   6 //     Christian Niephaus <cni@berlios.de>
   7 //
   8 // This program is free software; you can redistribute it and/or modify
   9 // it under the terms of the GNU General Public License as published by
  10 // the Free Software Foundation; either version 2 of the License, or
  11 // (at your option) any later version.
  12 //
  13 // This program is distributed in the hope that it will be useful,
  14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16 // GNU General Public License for more details.
  17 //
  18 // You should have received a copy of the GNU General Public License
  19 // along with this program; if not, write to the
  20 // Free Software Foundation, Inc.,
  21 // 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23 // Definition of RadiotapPacket non-inline non-template functions
  24
  25 #include "RadiotapPacket.hh"
  26 //#include "RadiotapPacket.ih"
  27
  28 // Custom includes
  29 #include "WLANPacket.hh"
  30 #include <boost/io/ios_state.hpp>
  31 #include <memory.h>
  32
  33 extern "C" {
  34 #   include "radiotap/radiotap_iter.h"
  35 }
  36
  37 #define prefix_
  38 ///////////////////////////////cc.p//////////////////////////////////////
  39
  40 ///////////////////////////////////////////////////////////////////////////
  41 // Offset table management
  42
  43 prefix_ senf::RadiotapPacketParser::OffsetTable &
  44 senf::RadiotapPacketParser::offsetTable(boost::uint32_t presentFlags)
  45 {
  46     typedef std::map<boost::uint32_t, OffsetTable> OffsetMap;
  47     static OffsetMap offsetMap;
  48
  49     OffsetMap::iterator i (offsetMap.find(presentFlags));
  50     if (i == offsetMap.end())
  51         i = offsetMap.insert(std::make_pair(presentFlags, OffsetTable())).first;
  52     return i->second;
  53 }
  54
  55 prefix_ void senf::RadiotapPacketParser::parseOffsetTable(boost::uint8_t * data, int maxLength,
  56                                                           OffsetTable & table)
  57 {
  58     struct ieee80211_radiotap_iterator iter;
  59     ieee80211_radiotap_iterator_init(&iter,
  60                                      (struct ieee80211_radiotap_header *)data,
  61                                      maxLength,
  62                                      0);
  63     unsigned size (8u);
  64     while (ieee80211_radiotap_iterator_next(&iter) == 0) {
  65         if (iter.is_radiotap_ns &&
  66             iter.this_arg_index <= int(senf::RadiotapPacketParser::MAX_INDEX))
  67             table[iter.this_arg_index] = iter.this_arg - data;
  68         // We need to set size here in the loop since the iter fields are only valid
  69         // when at least one present bit is set ...
  70         size = iter.this_arg - data + iter.this_arg_size;
  71     }
  72     table[MAX_INDEX+1] = size;
  73 }
  74
  75 prefix_ void senf::RadiotapPacketParser::buildOffsetTable(boost::uint32_t presentFlags,
  76                                                           OffsetTable & table)
  77 {
  78     SENF_ASSERT(!(presentFlags & ( (1<<IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE) |
  79                                    (1<<IEEE80211_RADIOTAP_VENDOR_NAMESPACE) |
  80                                    (1<<IEEE80211_RADIOTAP_EXT) )),
  81                 "Extended or vendor fields not supported");
  82
  83     struct ieee80211_radiotap_header header;
  84     memset(&header, 0, sizeof(header));
  85     // header.it_version = 0;
  86
  87     // Iterating this packet will generate invalid addresses but we don't care since neither
  88     // radiotap.c nor we will ever dereference those pointers, we just calculate the offsets.
  89     // This works, as long as we don't support extension headers ...
  90     header.it_len = 0xFFFF;
  91     header.it_present = presentFlags;
  92
  93     parseOffsetTable((boost::uint8_t*)&header, header.it_len, table);
  94 }
  95
  96 ///////////////////////////////////////////////////////////////////////////
  97 // senf::RadiotapPacketParser
  98
  99 unsigned const senf::RadiotapPacketParser_Header::FIELD_SIZE[] = {
 100     8, 1, 1, 4, 2, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 1, 1 };
 101
 102 prefix_ senf::UInt32Parser senf::RadiotapPacketParser::init_fcs()
 103 {
 104     if (!has_fcs()) {
 105         protect(), data().insert(data().end(), 4u, 0u);
 106         init_flags().fcsAtEnd_() = true;
 107     }
 108     return fcs();
 109 }
 110
 111 prefix_ void senf::RadiotapPacketParser::disable_fcs()
 112 {
 113     if (has_fcs()) {
 114         validate(RadiotapPacketParser_Header::fixed_bytes+4);
 115         data().erase(data().end()-4, data().end());
 116         flags().fcsAtEnd_() = false;
 117     }
 118 }
 119
 120 prefix_ senf::RadiotapPacketParser::OffsetTable const &
 121 senf::RadiotapPacketParser::getTable(boost::uint32_t presentFlags)
 122     const
 123 {
 124     OffsetTable & table(offsetTable(presentFlags));
 125     if (! table[MAX_INDEX+1])
 126         buildOffsetTable(presentFlags, table);
 127     return table;
 128 }
 129
 130 prefix_ void senf::RadiotapPacketParser::insertRemoveBytes(unsigned from , unsigned to, int bytes)
 131 {
 132     data_iterator b (i() + from);
 133     data_iterator e (i() + to);
 134     if (bytes >= 0) {
 135         // Insert some bytes cleaning the old bytes to 0 first
 136         std::fill(b, e,  0u);
 137         if (bytes > 0)
 138             // need to protect the parser since data().insert() invalidates iterators
 139             protect(), data().insert(e, bytes, 0u);
 140     }
 141     else { // bytes < 0
 142         // Remove some bytes ...
 143         // remember: bytes is negative ...
 144         if (b < e + bytes)
 145             std::fill(b, e + bytes, 0u);
 146         data().erase(e + bytes, e);
 147     }
 148 }
 149
 150 prefix_ void senf::RadiotapPacketParser::updatePresentFlags(boost::uint32_t flags)
 151 {
 152     if (flags == presentFlags())
 153         return;
 154     validate(bytes());
 155
 156     OffsetTable const & oldTable (currentTable());
 157     OffsetTable const & newTable (getTable(flags));
 158     unsigned b (RadiotapPacketParser_Header::fixed_bytes);
 159     int cumulativeNewBytes (0);
 160
 161     for (unsigned index (0); index <= MAX_INDEX; ++index) {
 162         // Skip any unchanged fields
 163         for (; index <= MAX_INDEX+1
 164                  && ((oldTable[index] == 0 && newTable[index] == 0)
 165                      || (oldTable[index]+cumulativeNewBytes == newTable[index])); ++index)
 166             if (newTable[index] != 0)
 167                 b = newTable[index] + FIELD_SIZE[index];
 168         if (index > MAX_INDEX+1)
 169             break;
 170         // Now skip over all changed fields
 171         // (The condition index <= MAX_INDEX is not needed here since the last
 172         // table entry MAX_INDEX+1 is always != 0 in both tables)
 173         for (; ! (oldTable[index]!=0 && newTable[index]!=0); ++index) ;
 174         // index now either points to
 175         // a) an entry set in both tables
 176         // b) at the end of the table which contains the total length
 177         // (remember: the table has a size of MAX_INDEX+2 entries !!)
 178         // in both cases, the difference between the new and old size
 179         // is found from the difference between the old and the new table
 180         // entry
 181         int newBytes (newTable[index] - oldTable[index] - cumulativeNewBytes);
 182         insertRemoveBytes(b, oldTable[index] + cumulativeNewBytes, newBytes);
 183         cumulativeNewBytes += newBytes;
 184         b = newTable[index] + FIELD_SIZE[index];
 185     }
 186     presentFlags() = flags;
 187     currentTable_ = &newTable;
 188 }
 189
 190 ///////////////////////////////////////////////////////////////////////////
 191 // senf::RadiotapPacketType
 192
 193 prefix_ void senf::RadiotapPacketType::dump(packet p, std::ostream &os)
 194 {
 195     boost::io::ios_all_saver ias(os);
 196     os << "Radiotap:\n"
 197        << senf::fieldName("version") << unsigned(p->version()) << '\n'
 198        << senf::fieldName("length")  << unsigned(p->length()) << '\n';
 199
 200 #   define FIELD(name, sign, desc)                                      \
 201         if (p->name ## Present())                                       \
 202             os << senf::fieldName(desc) << sign(p->name()) << '\n';
 203
 204 #   define ENTER(name)                                                  \
 205         if (p->name ## Present()) {                                     \
 206             packet::Parser::name ## _t subparser (p->name());
 207
 208 #   define SUBFIELD(name, sign, desc)                                   \
 209         os << senf::fieldName(desc) << sign(subparser.name()) << '\n';
 210
 211 #   define LEAVE()                                                      \
 212         }
 213
 214 #   define START_FLAGS(desc)                                            \
 215         os << senf::fieldName(desc);
 216
 217 #   define FLAG(name, desc)                                             \
 218         if (subparser.name()) os << desc " "
 219
 220 #   define END_FLAGS()                                                  \
 221         os << '\n';
 222
 223     FIELD           ( tsft,              boost::uint64_t, "MAC timestamp"        );
 224     ENTER           ( flags                                                      );
 225       START_FLAGS   (                                     "flags"                );
 226         FLAG        (     shortGI,                            "ShortGI"          );
 227         FLAG        (     badFCS,                             "BadFCS"           );
 228         FLAG        (     fcsAtEnd,                           "FCSatEnd"         );
 229         FLAG        (     fragmentation,                      "Frag"             );
 230         FLAG        (     wep,                                "WEP"              );
 231         FLAG        (     shortPreamble,                      "ShortPreamble"    );
 232         FLAG        (     cfp,                                "CFP"              );
 233       END_FLAGS     (                                                            );
 234     LEAVE           (                                                            );
 235     FIELD           ( rate,              unsigned,        "rate"                 );
 236     ENTER           ( channelOptions                                             );
 237       SUBFIELD      (     freq,          unsigned,        "channel frequency"    );
 238       START_FLAGS   (                                     "channel flags"        );
 239         FLAG        (     flag2ghz,                           "2GHz"             );
 240         FLAG        (     ofdm,                               "OFDM"             );
 241         FLAG        (     cck,                                "CCK"              );
 242         FLAG        (     turbo,                              "Turbo"            );
 243         FLAG        (     quarterRateChannel,                 "Rate/4"           );
 244         FLAG        (     halfRateChannel,                    "Rate/2"           );
 245         FLAG        (     gsm,                                "GSM"              );
 246         FLAG        (     staticTurbo,                        "StaticTurbo"      );
 247         FLAG        (     gfsk,                               "GFSK"             );
 248         FLAG        (     cckOfdm,                            "CCK+OFDM"         );
 249         FLAG        (     passive,                            "Passive"          );
 250         FLAG        (     flag5ghz,                           "5GHz"             );
 251       END_FLAGS     (                                                            );
 252     LEAVE           (                                                            );
 253     FIELD           ( fhss,              unsigned,        "FHSS"                 );
 254     FIELD           ( dbmAntennaSignal,  signed,          "antenna signal (dBm)" );
 255     FIELD           ( dbmAntennaNoise,   signed,          "antenna noise (dBm)"  );
 256     FIELD           ( lockQuality,       unsigned,        "lock quality"         );
 257     FIELD           ( txAttenuation,     unsigned,        "tx attenuation"       );
 258     FIELD           ( dbTxAttenuation,   unsigned,        "tx attenuation (dB)"  );
 259     FIELD           ( dbmTxAttenuation,  signed,          "tx attenuation (dBm)" );
 260     FIELD           ( antenna,           unsigned,        "antenna"              );
 261     FIELD           ( dbAntennaSignal,   unsigned,        "antenna signal (dB)"  );
 262     FIELD           ( dbAntennaNoise,    unsigned,        "antenna noise (dB)"   );
 263     ENTER           ( rxFlags                                                    );
 264       START_FLAGS   (                                     "rx flags"             );
 265         FLAG        (     badPlcp,                            "BadPLCP"          );
 266       END_FLAGS     (                                                            );
 267     LEAVE           (                                                            );
 268     ENTER           ( txFlags                                                    );
 269       START_FLAGS   (                                     "tx flags"             );
 270         FLAG        (     fail,                               "Fail"             );
 271         FLAG        (     txRts,                              "RTS"              );
 272         FLAG        (     txCts,                              "CTS"              );
 273       END_FLAGS     (                                                            );
 274     LEAVE           (                                                            );
 275     FIELD           ( rtsRetries,        unsigned,        "rts retries"          );
 276     FIELD           ( dataRetries,       unsigned,        "data retries"         );
 277
 278     if (p->flagsPresent() && p->flags().fcsAtEnd())
 279         os << senf::fieldName("fcs") << unsigned(p->fcs()) << '\n';
 280
 281 #   undef END_FLAGS
 282 #   undef FLAG
 283 #   undef START_FLAGS
 284 #   undef LEAVE
 285 #   undef SUBFIELD
 286 #   undef ENTER
 287 #   undef FIELD
 288 }
 289
 290
 291 prefix_ void senf::RadiotapPacketType::finalize(packet p)
 292 {
 293     ///\fixme Is this really correct ? shouldn't I use nextPacket.begin() - begin() here ?
 294     p->length() << p->calculateSize();
 295 }
 296
 297 prefix_ senf::PacketInterpreterBase::factory_t senf::RadiotapPacketType::nextPacketType(packet p)
 298 {
 299     static factory_t frameTypeFactory[] = { WLANPacket_MgtFrame::factory(),
 300                                             WLANPacket_CtrlFrame::factory(),
 301                                             WLANPacket_DataFrame::factory(),
 302                                             no_factory() };
 303     return frameTypeFactory[p->frameType()];
 304 }
 305
 306 prefix_ senf::RadiotapPacketType::optional_range
 307 senf::RadiotapPacketType::nextPacketRange(packet p)
 308 {
 309     size_type h (senf::bytes(p.parser()));
 310     size_type t (p->flagsPresent() && p->flags().fcsAtEnd() ? 4 : 0);
 311     return p.size() <= h+t
 312         ? no_range()
 313         : optional_range( range(p.data().begin() + h, p.data().end() - t) );
 314 }
 315
 316 ///////////////////////////////cc.e////////////////////////////////////////
 317 #undef prefix_
 318
 319 \f
 320 // Local Variables:
 321 // mode: c++
 322 // fill-column: 100
 323 // c-file-style: "senf"
 324 // indent-tabs-mode: nil
 325 // ispell-local-dictionary: "american"
 326 // compile-command: "scons -u test"
 327 // comment-column: 40
 328 // End: