include/wipal/phy/rtap_header.hxx

/*
 * WiPal - A library and a set of tools to manipulate wireless traces.
 * Copyright (C) 2007, 2008, 2009  Universite Pierre et Marie Curie - Paris 6
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 * MA  02110-1301  USA
 *
 * Author: Thomas Claveirole <thomas.claveirole@lip6.fr>
 */
#ifndef RTAP_HEADER_HXX_
# define RTAP_HEADER_HXX_

# include "rtap_header.hh"

# include <stdexcept>
# include <cstring>

# include <wipal/tool/endianness.hh>

namespace wpl
{

  namespace rtap
  {

    inline
    size_t
    header::len_impl(size_t caplen, bool) const
    {
      // Unknown RTAP header version.
      if (caplen and it_version)
        return caplen + 1;

      // Truncated RTAP header.
      if (caplen < sizeof (rtap::header))
        return caplen + 1;

      return tool::extract_little_endian_short_u(it_len);
    }

    inline
    header::time_type
    header::time_get_impl(bool) const
    {
      return tsft_get(tool::extract_little_endian_short_u(it_len));
    }

    inline
    void
    header::time_set_impl(const time_type& t, bool)
    {
      const size_t      len = tool::extract_little_endian_short_u(it_len);
      void* const               f = first_field(len);
      const uint64_t    v = t.get_impl();

      if (not tool::need_swap(tool::end::little, false))
        *static_cast<uint64_t*> (f) = v;
      else
        {
          const uint32_t  lo = tool::extract_swapped_long_u(v & uint32_t (-1));
          const uint32_t  hi = tool::extract_swapped_long_u(v >> 32);
          uint32_t* const f32 = static_cast<uint32_t*> (f);

          f32[0] = lo;
          f32[1] = hi;
        }
    }

    template <class S1, class S2>
    bool
    header::eq_time(const pkt::packet<S1>&              lhs,
                    const pkt::packet<S2>&              rhs,
                    const unsigned                      prec,
                    const tool::end::endianness         )
    {
      const size_t              lhs_caplen = lhs.meta().caplen;
      const size_t              rhs_caplen = rhs.meta().caplen;

      const header* const       lhs_phy = (static_cast<const header*>
                                           (lhs.bytes()));
      const header* const       rhs_phy = (static_cast<const header*>
                                           (rhs.bytes()));

      if (lhs_phy->len(lhs_caplen, false) > lhs_caplen or
          rhs_phy->len(rhs_caplen, false) > rhs_caplen)
        return false;

      return std::abs(int64_t (lhs_phy->tsft_get(lhs_caplen) -
                               rhs_phy->tsft_get(rhs_caplen))) <= prec;
    }

# define WP_diff(P1, P2)  (reinterpret_cast<const uint8_t*> (P2) -      \
                           reinterpret_cast<const uint8_t*> (P1))

    inline
    const void*
    header::first_field(size_t caplen) const
    {
      const uint32_t* p = &it_present;

      while (WP_diff(this, p + 1) < ptrdiff_t (caplen) and
             tool::extract_little_endian_long_u(*p) & ext)
        ++p;
      return WP_diff(this, p + 1) >= ptrdiff_t (caplen) ? 0 : p + 1;
    }

    inline
    void*
    header::first_field(size_t caplen)
    {
      uint32_t* p = &it_present;

      while (WP_diff(this, p + 1) < ptrdiff_t (caplen) and
             tool::extract_little_endian_long_u(*p) & ext)
        ++p;
      return WP_diff(this, p + 1) >= ptrdiff_t (caplen) ? 0 : p + 1;
    }

# undef WP_diff

    inline
    uint64_t
    header::tsft_get(size_t caplen) const
    {
      if (not it_present & tsft)
        throw std::invalid_argument ("Radiotap header has no TSFT field");

      if (caplen < sizeof (rtap::header) + 8)
        throw std::invalid_argument ("Truncated or inconsistent RTAP header "
                                     "(length too short w.r.t. present fields)");

      const void* const f = first_field(caplen);

      assert(f);
      if (not tool::need_swap(tool::end::little, false))
        return *static_cast<const uint64_t*> (f);
      else
        {
          const uint32_t* const f32 = static_cast<const uint32_t*> (f);

          const uint32_t        lo = tool::extract_little_endian_long_u(f32[0]);
          const uint32_t        hi = tool::extract_little_endian_long_u(f32[1]);

          return (uint64_t (hi) << 32) | lo;
        }
    }

  } // End of namespace wpl::rtap.

} // End of namespace wpl.

#endif // ! RTAP_HEADER_HXX_

---