16a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvistpackage com.android.server.wifi.anqp; 26a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 36a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvistimport java.net.ProtocolException; 46a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvistimport java.nio.ByteBuffer; 56a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 66a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist/** 76a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * Holds an AP Geospatial Location ANQP Element, as specified in IEEE802.11-2012 section 86a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * 8.4.4.12. 96a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * <p/> 106a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * <p> 116a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * Section 8.4.2.24.10 of the IEEE802.11-2012 specification refers to RFC-3825 for the format of the 126a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * Geospatial location information. RFC-3825 has subsequently been obsoleted by RFC-6225 which 136a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * defines the same basic binary format for the DHCPv4 payload except that a few unused bits of the 146a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * Datum field have been reserved for other uses. 156a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * </p> 166a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * <p/> 176a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * <p> 186a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * RFC-3825 defines a resolution field for each of latitude, longitude and altitude as "the number 196a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * of significant bits" of precision in the respective values and implies through examples and 206a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * otherwise that the non-significant bits should be simply disregarded and the range of values are 216a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * calculated as the numeric interval obtained by varying the range of "insignificant bits" between 226a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * its extremes. As a simple example, consider the value 33 as a simple 8-bit number with three 236a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * significant bits: 33 is 00100001 binary and the leading 001 are the significant bits. With the 246a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * above definition, the range of numbers are [32,63] with 33 asymmetrically located at the low end 256a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * of the interval. In a more realistic setting an instrument, such as a GPS, would most likely 266a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * deliver measurements with a gaussian distribution around the exact value, meaning it is more 276a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * reasonable to assume the value as a "center" value with a symmetric uncertainty interval. 286a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * RFC-6225 redefines the "resolution" from RFC-3825 with an "uncertainty" value with these 296a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * properties, which is also the definition suggested here. 306a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * </p> 316a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * <p/> 326a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * <p> 336a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * The res fields provides the resolution as the exponent to a power of two, 346a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * e.g. 8 means 2^8 = +/- 256, 0 means 2^0 = +/- 1 and -7 means 2^-7 +/- 0.00781250. 356a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * Unknown resolution is indicated by not setting the respective resolution field in the RealValue. 366a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * </p> 376a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist */ 386a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvistpublic class GEOLocationElement extends ANQPElement { 396a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public enum AltitudeType {Unknown, Meters, Floors} 406a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 416a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public enum Datum {Unknown, WGS84, NAD83Land, NAD83Water} 426a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 436a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int ELEMENT_ID = 123; // ??? 446a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int GEO_LOCATION_LENGTH = 16; 456a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 466a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int LL_FRACTION_SIZE = 25; 476a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int LL_WIDTH = 34; 486a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int ALT_FRACTION_SIZE = 8; 496a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int ALT_WIDTH = 30; 506a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int RES_WIDTH = 6; 516a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int ALT_TYPE_WIDTH = 4; 526a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final int DATUM_WIDTH = 8; 536a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 546a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final RealValue mLatitude; 556a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final RealValue mLongitude; 566a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final RealValue mAltitude; 576a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final AltitudeType mAltitudeType; 586a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final Datum mDatum; 596a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 606a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public static class RealValue { 616a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final double mValue; 626a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final boolean mResolutionSet; 636a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final int mResolution; 646a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 656a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public RealValue(double value) { 666a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mValue = value; 676a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mResolution = Integer.MIN_VALUE; 686a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mResolutionSet = false; 696a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 706a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 716a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public RealValue(double value, int resolution) { 726a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mValue = value; 736a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mResolution = resolution; 746a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mResolutionSet = true; 756a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 766a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 776a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public double getValue() { 786a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return mValue; 796a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 806a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 816a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public boolean isResolutionSet() { 826a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return mResolutionSet; 836a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 846a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 856a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public int getResolution() { 866a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return mResolution; 876a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 886a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 896a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist @Override 906a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public String toString() { 916a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist StringBuilder sb = new StringBuilder(); 926a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist sb.append(String.format("%f", mValue)); 936a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist if (mResolutionSet) { 946a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist sb.append("+/-2^").append(mResolution); 956a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 966a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return sb.toString(); 976a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 986a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 996a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1006a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public GEOLocationElement(Constants.ANQPElementType infoID, ByteBuffer payload) 1016a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist throws ProtocolException { 1026a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist super(infoID); 1036a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1046a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist payload.get(); 1056a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int locLength = payload.get() & Constants.BYTE_MASK; 1066a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1076a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist if (locLength != GEO_LOCATION_LENGTH) { 1086a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist throw new ProtocolException("GeoLocation length field value " + locLength + 1096a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist " incorrect, expected 16"); 1106a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1116a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist if (payload.remaining() != GEO_LOCATION_LENGTH) { 1126a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist throw new ProtocolException("Bad buffer length " + payload.remaining() + 1136a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist ", expected 16"); 1146a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1156a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1166a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist ReverseBitStream reverseBitStream = new ReverseBitStream(payload); 1176a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1186a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int rawLatRes = (int) reverseBitStream.sliceOff(RES_WIDTH); 1196a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist double latitude = 1206a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist fixToFloat(reverseBitStream.sliceOff(LL_WIDTH), LL_FRACTION_SIZE, LL_WIDTH); 1216a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1226a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mLatitude = rawLatRes != 0 ? 1236a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist new RealValue(latitude, bitsToAbsResolution(rawLatRes, LL_WIDTH, 1246a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist LL_FRACTION_SIZE)) : 1256a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist new RealValue(latitude); 1266a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1276a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int rawLonRes = (int) reverseBitStream.sliceOff(RES_WIDTH); 1286a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist double longitude = 1296a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist fixToFloat(reverseBitStream.sliceOff(LL_WIDTH), LL_FRACTION_SIZE, LL_WIDTH); 1306a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1316a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mLongitude = rawLonRes != 0 ? 1326a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist new RealValue(longitude, bitsToAbsResolution(rawLonRes, LL_WIDTH, 1336a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist LL_FRACTION_SIZE)) : 1346a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist new RealValue(longitude); 1356a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1366a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int altType = (int) reverseBitStream.sliceOff(ALT_TYPE_WIDTH); 1376a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mAltitudeType = altType < AltitudeType.values().length ? 1386a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist AltitudeType.values()[altType] : 1396a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist AltitudeType.Unknown; 1406a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1416a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int rawAltRes = (int) reverseBitStream.sliceOff(RES_WIDTH); 1426a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist double altitude = fixToFloat(reverseBitStream.sliceOff(ALT_WIDTH), ALT_FRACTION_SIZE, 1436a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist ALT_WIDTH); 1446a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1456a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mAltitude = rawAltRes != 0 ? 1466a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist new RealValue(altitude, bitsToAbsResolution(rawAltRes, ALT_WIDTH, 1476a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist ALT_FRACTION_SIZE)) : 1486a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist new RealValue(altitude); 1496a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1506a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int datumValue = (int) reverseBitStream.sliceOff(DATUM_WIDTH); 1516a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mDatum = datumValue < Datum.values().length ? Datum.values()[datumValue] : Datum.Unknown; 1526a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1536a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1546a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public RealValue getLatitude() { 1556a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return mLatitude; 1566a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1576a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1586a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public RealValue getLongitude() { 1596a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return mLongitude; 1606a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1616a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1626a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public RealValue getAltitude() { 1636a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return mAltitude; 1646a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1656a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1666a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public AltitudeType getAltitudeType() { 1676a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return mAltitudeType; 1686a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1696a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1706a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public Datum getDatum() { 1716a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return mDatum; 1726a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1736a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1746a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist @Override 1756a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist public String toString() { 17677f2b82a2e80af8da52c22d69a76def6d4209757Jan Nordqvist return "GEOLocation{" + 1776a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist "mLatitude=" + mLatitude + 1786a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist ", mLongitude=" + mLongitude + 1796a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist ", mAltitude=" + mAltitude + 1806a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist ", mAltitudeType=" + mAltitudeType + 1816a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist ", mDatum=" + mDatum + 1826a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist '}'; 1836a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1846a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1856a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static class ReverseBitStream { 1866a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1876a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final byte[] mOctets; 1886a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private int mBitoffset; 1896a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1906a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private ReverseBitStream(ByteBuffer octets) { 1916a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mOctets = new byte[octets.remaining()]; 1926a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist octets.get(mOctets); 1936a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 1946a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 1956a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private long sliceOff(int bits) { 1966a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist final int bn = mBitoffset + bits; 1976a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int remaining = bits; 1986a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist long value = 0; 1996a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2006a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist while (mBitoffset < bn) { 2016a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int sbit = mBitoffset & 0x7; // Bit #0 is MSB, inclusive 2026a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int octet = mBitoffset >>> 3; 2036a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2046a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist // Copy the minimum of what's to the right of sbit 2056a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist // and how much more goes to the target 2066a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int width = Math.min(Byte.SIZE - sbit, remaining); 2076a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2086a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist value = (value << width) | getBits(mOctets[octet], sbit, width); 2096a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2106a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist mBitoffset += width; 2116a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist remaining -= width; 2126a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2136a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2146a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return value; 2156a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2166a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2176a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static int getBits(byte b, int b0, int width) { 2186a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int mask = (1 << width) - 1; 2196a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return (b >> (Byte.SIZE - b0 - width)) & mask; 2206a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2216a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2226a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2236a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static class BitStream { 2246a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2256a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private final byte[] data; 2266a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private int bitOffset; // bit 0 is MSB of data[0] 2276a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2286a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private BitStream(int octets) { 2296a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist data = new byte[octets]; 2306a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2316a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2326a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private void append(long value, int width) { 2336a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist System.out.printf("Appending %x:%d\n", value, width); 2346a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist for (int sbit = width - 1; sbit >= 0; ) { 2356a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int b0 = bitOffset >>> 3; 2366a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int dbit = bitOffset & 0x7; 2376a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2386a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int shr = sbit - 7 + dbit; 2396a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist int dmask = 0xff >>> dbit; 2406a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2416a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist if (shr >= 0) { 2426a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist data[b0] = (byte) ((data[b0] & ~dmask) | ((value >>> shr) & dmask)); 2436a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist bitOffset += Byte.SIZE - dbit; 2446a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist sbit -= Byte.SIZE - dbit; 2456a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } else { 2466a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist data[b0] = (byte) ((data[b0] & ~dmask) | ((value << -shr) & dmask)); 2476a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist bitOffset += sbit + 1; 2486a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist sbit = -1; 2496a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2506a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2516a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2526a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2536a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private byte[] getOctets() { 2546a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return data; 2556a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2566a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2576a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2586a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist static double fixToFloat(long value, int fractionSize, int width) { 2596a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist long sign = 1L << (width - 1); 2606a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist if ((value & sign) != 0) { 2616a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist value = -value; 2626a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return -(double) (value & (sign - 1)) / (double) (1L << fractionSize); 2636a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } else { 2646a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return (double) (value & (sign - 1)) / (double) (1L << fractionSize); 2656a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2666a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2676a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2686a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static long floatToFix(double value, int fractionSize, int width) { 2696a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return Math.round(value * (1L << fractionSize)) & ((1L << width) - 1); 2706a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2716a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2726a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static final double LOG2_FACTOR = 1.0 / Math.log(2.0); 2736a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2746a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist /** 2756a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * Convert an absolute variance value into absolute resolution representation, 2766a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * where the variance = 2^resolution. 2776a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * 2786a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @param variance The absolute variance 2796a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @return the absolute resolution. 2806a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist */ 2816a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static int getResolution(double variance) { 2826a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return (int) Math.ceil(Math.log(variance) * LOG2_FACTOR); 2836a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2846a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2856a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist /** 2866a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * Convert an absolute resolution, into the "number of significant bits" for the given fixed 2876a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * point notation as defined in RFC-3825 and refined in RFC-6225. 2886a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * 2896a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @param resolution absolute resolution given as 2^resolution. 2906a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @param fieldWidth Full width of the fixed point number used to represent the value. 2916a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @param fractionBits Number of fraction bits in the fixed point number used to represent the 2926a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * value. 2936a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @return The number of "significant bits". 2946a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist */ 2956a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static int absResolutionToBits(int resolution, int fieldWidth, int fractionBits) { 2966a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return fieldWidth - fractionBits - 1 - resolution; 2976a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 2986a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist 2996a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist /** 3006a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * Convert the protocol definition of "number of significant bits" into an absolute resolution. 3016a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * 3026a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @param bits The number of "significant bits" from the binary protocol. 3036a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @param fieldWidth Full width of the fixed point number used to represent the value. 3046a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @param fractionBits Number of fraction bits in the fixed point number used to represent the 3056a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * value. 3066a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist * @return The absolute resolution given as 2^resolution. 3076a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist */ 3086a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist private static int bitsToAbsResolution(long bits, int fieldWidth, int fractionBits) { 3096a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist return fieldWidth - fractionBits - 1 - (int) bits; 3106a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist } 3116a3903fed590e369b576bddbe1ae2d788768ddfeJan Nordqvist} 312