path: root/scripts/external_libs/scapy-2.3.1/python3/scapy/contrib/ppi_geotag.py

## This file is (hopefully) part of Scapy
## See http://www.secdev.org/projects/scapy for more informations
## <jellch@harris.com>
## This program is published under a GPLv2 license

# scapy.contrib.description = PPI GEOLOCATION
# scapy.contrib.status = loads


"""
PPI-GEOLOCATION tags
"""
import struct
from scapy.packet import *
from scapy.fields import *
from scapy.contrib.ppi import PPIGenericFldHdr,addPPIType

CURR_GEOTAG_VER = 2 #Major revision of specification

PPI_GPS     = 30002
PPI_VECTOR  = 30003
PPI_SENSOR  = 30004
PPI_ANTENNA = 30005
#The FixedX_Y Fields are used to store fixed point numbers in a variety of fields in the GEOLOCATION-TAGS specification
class Fixed3_6Field(LEIntField):
    def i2h(self, pkt, x):
        if x is not None:
            if (x < 0):
                warning("Fixed3_6: Internal value too negative: %d" % x)
                x = 0
            elif (x > 999999999):
                warning("Fixed3_6: Internal value too positive: %d" % x)
                x = 999999999
            x = x * 1e-6
        return x
    def h2i(self, pkt, x):
        if x is not None:
            if (x <= -0.5e-6):
                warning("Fixed3_6: Input value too negative: %.7f" % x)
                x = 0
            elif (x >= 999.9999995):
                warning("Fixed3_6: Input value too positive: %.7f" % x)
                x = 999.999999
            x = int(round(x * 1e6))
        return x
    def i2m(self, pkt, x):
        """Convert internal value to machine value"""
        if x is None:
            #Try to return zero if undefined
            x = self.h2i(pkt, 0)
        return x

    def i2repr(self,pkt,x):
        if x is None:
            y=0
        else:
            y=self.i2h(pkt,x)
        return "%3.6f"%(y)
class Fixed3_7Field(LEIntField):
    def i2h(self, pkt, x):
        if x is not None:
            if (x < 0):
                warning("Fixed3_7: Internal value too negative: %d" % x)
                x = 0
            elif (x > 3600000000):
                warning("Fixed3_7: Internal value too positive: %d" % x)
                x = 3600000000
            x = (x - 1800000000) * 1e-7
        return x
    def h2i(self, pkt, x):
        if x is not None:
            if (x <= -180.00000005):
                warning("Fixed3_7: Input value too negative: %.8f" % x)
                x = -180.0
            elif (x >= 180.00000005):
                warning("Fixed3_7: Input value too positive: %.8f" % x)
                x = 180.0
            x = int(round((x + 180.0) * 1e7))
        return x
    def i2m(self, pkt, x):
        """Convert internal value to machine value"""
        if x is None:
            #Try to return zero if undefined
            x = self.h2i(pkt, 0)
        return x
    def i2repr(self,pkt,x):
        if x is None:
            y=0
        else:
            y=self.i2h(pkt,x)
        return "%3.7f"%(y)

class Fixed6_4Field(LEIntField):
    def i2h(self, pkt, x):
        if x is not None:
            if (x < 0):
                warning("Fixed6_4: Internal value too negative: %d" % x)
                x = 0
            elif (x > 3600000000):
                warning("Fixed6_4: Internal value too positive: %d" % x)
                x = 3600000000
            x = (x - 1800000000) * 1e-4
        return x
    def h2i(self, pkt, x):
        if x is not None:
            if (x <= -180000.00005):
                warning("Fixed6_4: Input value too negative: %.5f" % x)
                x = -180000.0
            elif (x >= 180000.00005):
                warning("Fixed6_4: Input value too positive: %.5f" % x)
                x = 180000.0
            x = int(round((x + 180000.0) * 1e4))
        return x
    def i2m(self, pkt, x):
        """Convert internal value to machine value"""
        if x is None:
            #Try to return zero if undefined
            x = self.h2i(pkt, 0)
        return x
    def i2repr(self,pkt,x):
        if x is None:
            y=0
        else:
            y=self.i2h(pkt,x)
        return "%6.4f"%(y)
#The GPS timestamps fractional time counter is stored in a 32-bit unsigned ns counter.
#The ept field is as well,
class NSCounter_Field(LEIntField):
    def i2h(self, pkt, x): #converts nano-seconds to seconds for output
        if x is not None:
            if (x < 0):
                warning("NSCounter_Field: Internal value too negative: %d" % x)
                x = 0
            elif (x >= 2**32):
                warning("NSCounter_Field: Internal value too positive: %d" % x)
                x = 2**32-1
            x = (x / 1e9)
        return x
    def h2i(self, pkt, x): #converts input in seconds into nano-seconds for storage
        if x is not None:
            if (x < 0):
                warning("NSCounter_Field: Input value too negative: %.10f" % x)
                x = 0
            elif (x >= (2**32) / 1e9):
                warning("NSCounter_Field: Input value too positive: %.10f" % x)
                x = (2**32-1) / 1e9
            x = int(round((x * 1e9)))
        return x
    def i2repr(self,pkt,x):
        if x is None:
            y=0
        else:
            y=self.i2h(pkt,x)
        return "%1.9f"%(y)

class UTCTimeField(IntField):
    def __init__(self, name, default, epoch=time.gmtime(0), strf="%a, %d %b %Y %H:%M:%S +0000"):
        IntField.__init__(self, name, default)
        self.epoch = epoch
        self.delta = time.mktime(epoch) - time.mktime(time.gmtime(0))
        self.strf = strf
    def i2repr(self, pkt, x):
        if x is None:
            x = 0
        x = int(x) + self.delta
        t = time.strftime(self.strf, time.gmtime(x))
        return "%s (%d)" % (t, x)

class LETimeField(UTCTimeField,LEIntField):
    def __init__(self, name, default, epoch=time.gmtime(0), strf="%a, %d %b %Y %H:%M:%S +0000"):
        LEIntField.__init__(self, name, default)
        self.epoch = epoch
        self.delta = time.mktime(epoch) - time.mktime(time.gmtime(0))
        self.strf = strf

class SignedByteField(Field):
    def __init__(self, name, default):
        Field.__init__(self, name, default, "b")
    def randval(self):
        return RandSByte()

class XLEShortField(LEShortField,XShortField):
    def i2repr(self, pkt, x):
        return XShortField.i2repr(self, pkt, x)

class XLEIntField(LEIntField,XIntField):
    def i2repr(self, pkt, x):
        return XIntField.i2repr(self, pkt, x)

class GPSTime_Field(LETimeField):
    def __init__(self, name, default):
        return LETimeField.__init__(self, name, default, strf="%a, %d %b %Y %H:%M:%S UTC")

class VectorFlags_Field(XLEIntField):
    """Represents te VectorFlags field. Handles the RelativeTo:sub-field"""
    _fwdstr   = "DefinesForward"
    _resmask  = 0xfffffff8
    _relmask  = 0x6
    _relnames = ["RelativeToForward", "RelativeToEarth", "RelativeToCurrent", "RelativeToReserved"]
    _relvals  = [0x00, 0x02, 0x04, 0x06]
    def i2repr(self, pkt, x):
        if x is None:
            return str(x)
        r = []
        if (x & 0x1):
            r.append(self._fwdstr)
        i = (x & self._relmask) >> 1
        r.append(self._relnames[i])
        i = x & self._resmask
        if (i):
            r.append("ReservedBits:%08X" % i)
        sout = "+".join(r)
        return sout
    def any2i(self, pkt, x):
        if type(x) is str:
            r = x.split("+")
            y = 0
            for value in r:
                if (value == self._fwdstr):
                    y |= 0x1
                elif (value in self._relnames):
                    i = self._relnames.index(value)
                    y &= (~self._relmask)
                    y |= self._relvals[i]
                else:
                    #logging.warning("Unknown VectorFlags Argument: %s" % value)
                    pass
        else:
            y = x
        #print "any2i: %s --> %s" % (str(x), str(y))
        return y

class HCSIFlagsField(FlagsField):
    """ A FlagsField where each bit/flag turns a conditional field on or off.
    If the value is None when building a packet, i2m() will check the value of
    every field in self.names.  If the field's value is not None, the corresponding
    flag will be set. """
    def i2m(self, pkt, val):
        if val is None:
            val = 0
            if (pkt):
                for i in range(len(self.names)):
                    name = self.names[i][0]
                    value = pkt.getfieldval(name)
                    if value is not None:
                        val |= 1 << i
        return val

class HCSINullField(StrFixedLenField):
    def __init__(self, name, default):
        return StrFixedLenField.__init__(self, name, default, length=0)

class HCSIDescField(StrFixedLenField):
    def __init__(self, name, default):
        return StrFixedLenField.__init__(self, name, default, length=32)

class HCSIAppField(StrFixedLenField):
    def __init__(self, name, default):
        return StrFixedLenField.__init__(self, name, default, length=60)

def _FlagsList(myfields):
    flags = []
    for i in range(32):
        flags.append("Reserved%02d" % i)
    for i in myfields.keys():
        flags[i] = myfields[i]
    return flags

# Define all geolocation-tag flags lists
_hcsi_gps_flags = _FlagsList({0:"No Fix Available", 1:"GPS", 2:"Differential GPS",
                              3:"Pulse Per Second", 4:"Real Time Kinematic",
                              5:"Float Real Time Kinematic", 6:"Estimated (Dead Reckoning)",
                              7:"Manual Input", 8:"Simulation"})

#_hcsi_vector_flags = _FlagsList({0:"ForwardFrame", 1:"RotationsAbsoluteXYZ", 5:"OffsetFromGPS_XYZ"})
#This has been replaced with the VectorFlags_Field class, in order to handle the RelativeTo:subfield

_hcsi_vector_char_flags = _FlagsList({0:"Antenna", 1:"Direction of Travel",
                                      2:"Front of Vehicle", 3:"Angle of Arrival", 4:"Transmitter Position",
                                      8:"GPS Derived", 9:"INS Derived", 10:"Compass Derived",
                                     11:"Acclerometer Derived", 12:"Human Derived"})

_hcsi_antenna_flags = _FlagsList({ 1:"Horizontal Polarization",     2:"Vertical Polarization",
                                   3:"Circular Polarization Left",  4:"Circular Polarization Right",
                                  16:"Electronically Steerable",   17:"Mechanically Steerable"})

""" HCSI PPI Fields are similar to RadioTap.  A mask field called "present" specifies if each field
is present.  All other fields are conditional.  When dissecting a packet, each field is present if
"present" has the corresponding bit set.  When building a packet, if "present" is None, the mask is
set to include every field that does not have a value of None.  Otherwise, if the mask field is
not None, only the fields specified by "present" will be added to the packet.

To build each Packet type, build a list of the fields normally, excluding the present bitmask field.
The code will then construct conditional versions of each field and add the present field.
See GPS_Fields as an example. """

# Conditional test for all HCSI Fields
def _HCSITest(pkt, ibit, name):
    if pkt.present is None:
        return (pkt.getfieldval(name) is not None)
    return pkt.present & ibit

# Wrap optional fields in ConditionalField, add HCSIFlagsField
def _HCSIBuildFields(fields):
    names = [f.name for f in fields]
    cond_fields = [ HCSIFlagsField('present', None, -len(names), names)]
    for i in range(len(names)):
        ibit = 1 << i
        seval = "lambda pkt:_HCSITest(pkt,%s,'%s')" % (ibit, names[i])
        test = eval(seval)
        cond_fields.append(ConditionalField(fields[i], test))
    return cond_fields

class HCSIPacket(Packet):
    name = "PPI HCSI"
    fields_desc = [ LEShortField('pfh_type', None),
                    LEShortField('pfh_length', None),
                    ByteField('geotag_ver', CURR_GEOTAG_VER),
                    ByteField('geotag_pad', 0),
                    LEShortField('geotag_len', None)]
    def post_build(self, p, pay):
        if self.pfh_length is None:
            l = len(p) - 4
            sl = struct.pack('<H',l)
            p = p[:2] + sl + p[4:]
        if self.geotag_len is None:
            l_g = len(p) - 4
            sl_g = struct.pack('<H',l_g)
            p = p[:6] + sl_g + p[8:]
        p += pay
        return p
    def extract_padding(self, p):
        return "",p

#GPS Fields
GPS_Fields = [FlagsField("GPSFlags", None, -32, _hcsi_gps_flags),
              Fixed3_7Field("Latitude", None),
              Fixed3_7Field("Longitude", None),    Fixed6_4Field("Altitude", None),
              Fixed6_4Field("Altitude_g", None),   GPSTime_Field("GPSTime", None),
              NSCounter_Field("FractionalTime", None),  Fixed3_6Field("eph", None),
              Fixed3_6Field("epv", None),          NSCounter_Field("ept", None),
              HCSINullField("Reserved10", None),   HCSINullField("Reserved11", None),
              HCSINullField("Reserved12", None),   HCSINullField("Reserved13", None),
              HCSINullField("Reserved14", None),   HCSINullField("Reserved15", None),
              HCSINullField("Reserved16", None),   HCSINullField("Reserved17", None),
              HCSINullField("Reserved18", None),   HCSINullField("Reserved19", None),
              HCSINullField("Reserved20", None),   HCSINullField("Reserved21", None),
              HCSINullField("Reserved22", None),   HCSINullField("Reserved23", None),
              HCSINullField("Reserved24", None),   HCSINullField("Reserved25", None),
              HCSINullField("Reserved26", None),   HCSINullField("Reserved27", None),
              HCSIDescField("DescString", None),   XLEIntField("AppId", None),
              HCSIAppField("AppData", None),       HCSINullField("Extended", None)]

class GPS(HCSIPacket):
    name = "PPI GPS"
    fields_desc = [ LEShortField('pfh_type', PPI_GPS), #pfh_type
                    LEShortField('pfh_length', None), #pfh_len
                    ByteField('geotag_ver', CURR_GEOTAG_VER), #base_geotag_header.ver
                    ByteField('geotag_pad', 0), #base_geotag_header.pad
                    LEShortField('geotag_len', None)] + _HCSIBuildFields(GPS_Fields)


#Vector Fields
VEC_Fields = [VectorFlags_Field("VectorFlags", None),
              FlagsField("VectorChars", None, -32, _hcsi_vector_char_flags),
              Fixed3_6Field("Pitch", None),       Fixed3_6Field("Roll", None),
              Fixed3_6Field("Heading", None),     Fixed6_4Field("Off_X", None),
              Fixed6_4Field("Off_Y", None),       Fixed6_4Field("Off_Z", None),
              HCSINullField("Reserved08", None),  HCSINullField("Reserved09", None),
              HCSINullField("Reserved10", None),  HCSINullField("Reserved11", None),
              HCSINullField("Reserved12", None),  HCSINullField("Reserved13", None),
              HCSINullField("Reserved14", None),  HCSINullField("Reserved15", None),
              Fixed3_6Field("Err_Rot", None),     Fixed6_4Field("Err_Off", None),
              HCSINullField("Reserved18", None),  HCSINullField("Reserved19", None),
              HCSINullField("Reserved20", None),  HCSINullField("Reserved21", None),
              HCSINullField("Reserved22", None),  HCSINullField("Reserved23", None),
              HCSINullField("Reserved24", None),  HCSINullField("Reserved25", None),
              HCSINullField("Reserved26", None),  HCSINullField("Reserved27", None),
              HCSIDescField("DescString", None),  XLEIntField("AppId", None),
              HCSIAppField("AppData", None),      HCSINullField("Extended", None)]

class Vector(HCSIPacket):
    name = "PPI Vector"
    fields_desc = [ LEShortField('pfh_type', PPI_VECTOR), #pfh_type
                    LEShortField('pfh_length', None), #pfh_len
                    ByteField('geotag_ver', CURR_GEOTAG_VER), #base_geotag_header.ver
                    ByteField('geotag_pad', 0), #base_geotag_header.pad
                    LEShortField('geotag_len', None)] + _HCSIBuildFields(VEC_Fields)

#Sensor Fields
# http://www.iana.org/assignments/icmp-parameters
sensor_types= { 1   : "Velocity",
                2   : "Acceleration",
                3   : "Jerk",
                100 : "Rotation",
                101 : "Magnetic",
                1000: "Temperature",
                1001: "Barometer",
                1002: "Humidity",
                2000: "TDOA_Clock",
                2001: "Phase"
                }
SENS_Fields = [  LEShortEnumField('SensorType', None, sensor_types),
                 SignedByteField('ScaleFactor', None),
                 Fixed6_4Field('Val_X', None),
                 Fixed6_4Field('Val_Y', None),
                 Fixed6_4Field('Val_Z', None),
                 Fixed6_4Field('Val_T', None),
                 Fixed6_4Field('Val_E', None),
              HCSINullField("Reserved07", None),  HCSINullField("Reserved08", None),
              HCSINullField("Reserved09", None),  HCSINullField("Reserved10", None),
              HCSINullField("Reserved11", None),  HCSINullField("Reserved12", None),
              HCSINullField("Reserved13", None),  HCSINullField("Reserved14", None),
              HCSINullField("Reserved15", None),  HCSINullField("Reserved16", None),
              HCSINullField("Reserved17", None),  HCSINullField("Reserved18", None),
              HCSINullField("Reserved19", None),  HCSINullField("Reserved20", None),
              HCSINullField("Reserved21", None),  HCSINullField("Reserved22", None),
              HCSINullField("Reserved23", None),  HCSINullField("Reserved24", None),
              HCSINullField("Reserved25", None),  HCSINullField("Reserved26", None),
              HCSINullField("Reserved27", None),
              HCSIDescField("DescString", None),  XLEIntField("AppId", None),
              HCSIAppField("AppData", None),      HCSINullField("Extended", None)]

              

class Sensor(HCSIPacket):
    name = "PPI Sensor"
    fields_desc = [ LEShortField('pfh_type', PPI_SENSOR), #pfh_type
                    LEShortField('pfh_length', None), #pfh_len
                    ByteField('geotag_ver', CURR_GEOTAG_VER ), #base_geotag_header.ver
                    ByteField('geotag_pad', 0), #base_geotag_header.pad
                    LEShortField('geotag_len', None)] + _HCSIBuildFields(SENS_Fields)

# HCSIAntenna Fields
ANT_Fields = [FlagsField("AntennaFlags", None, -32, _hcsi_antenna_flags),
              ByteField("Gain", None),
              Fixed3_6Field("HorizBw", None),              Fixed3_6Field("VertBw", None),
              Fixed3_6Field("PrecisionGain",None),         XLEShortField("BeamID", None),
              HCSINullField("Reserved06", None),           HCSINullField("Reserved07", None),
              HCSINullField("Reserved08", None),           HCSINullField("Reserved09", None),
              HCSINullField("Reserved10", None),           HCSINullField("Reserved11", None),
              HCSINullField("Reserved12", None),           HCSINullField("Reserved13", None),
              HCSINullField("Reserved14", None),           HCSINullField("Reserved15", None),
              HCSINullField("Reserved16", None),           HCSINullField("Reserved17", None),
              HCSINullField("Reserved18", None),           HCSINullField("Reserved19", None),
              HCSINullField("Reserved20", None),           HCSINullField("Reserved21", None),
              HCSINullField("Reserved22", None),           HCSINullField("Reserved23", None),
              HCSINullField("Reserved24", None),           HCSINullField("Reserved25", None),
              HCSIDescField("SerialNumber", None),         HCSIDescField("ModelName", None),
              HCSIDescField("DescString", None),           XLEIntField("AppId", None),
              HCSIAppField("AppData", None),               HCSINullField("Extended", None)]

class Antenna(HCSIPacket):
    name = "PPI Antenna"
    fields_desc = [ LEShortField('pfh_type', PPI_ANTENNA), #pfh_type
                    LEShortField('pfh_length', None), #pfh_len
                    ByteField('geotag_ver', CURR_GEOTAG_VER), #base_geotag_header.ver
                    ByteField('geotag_pad', 0), #base_geotag_header.pad
                    LEShortField('geotag_len', None)] + _HCSIBuildFields(ANT_Fields)

addPPIType(PPI_GPS, GPS)
addPPIType(PPI_VECTOR, Vector)
addPPIType(PPI_SENSOR, Sensor)
addPPIType(PPI_ANTENNA,Antenna)