zircon/system/ulib/hid-parser/report.cc

// Copyright 2019 The Fuchsia Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <type_traits>

#include <hid-parser/report.h>
#include <hid-parser/units.h>

namespace hid {
namespace {

// This sign extends the |n_bits| of |data| and returns it as a full
// int32_t value. |n_bits| must be between [0, 31].
// Example: If the user had a 2's complement 5 bit number 0b11111 it
// would represent -1. To sign extend this to an int32_t the function
// would be called as SignExtendFromBits(0b11111, 5)
constexpr int32_t SignExtendFromNBits(uint32_t data, int32_t n_bits) {
  // Expression taken and simplified from:
  // http://graphics.stanford.edu/~seander/bithacks.html#VariableSignExtend

  // Zero out information about n_bits.
  data = data & ((1U << n_bits) - 1);
  // Do the sign extend.
  int32_t msb = 1U << (n_bits - 1);
  return static_cast<int32_t>((data ^ msb) - msb);
}

inline bool FieldFits(size_t report_len, const Attributes& attr) {
  return (attr.offset + attr.bit_sz) <= (report_len * 8);
}

// Extracts bits from a byte and returns them.
// |begin| is the starting bit: it starts at 0 and counts from LSB to MSB.
// The bits are placed at the beginning of return value.
// Make sure when this is called that (|begin| + |count|) <= 8.
// Example: ExtractBitsFromByte(1b00010100, 2, 3) returns 1b101.
inline uint8_t ExtractBitsFromByte(uint8_t val, uint32_t begin, uint8_t count) {
  uint8_t mask = static_cast<uint8_t>((0xFF >> (8 - count)) << begin);
  return static_cast<uint8_t>((val & mask) >> begin);
}

// Create a mask of set bits of a given |size| starting at |start_bit|.
// Eg. create_mask(2, 3) = 1b11100;
constexpr uint32_t create_mask(uint32_t start_bit, uint32_t size) {
  return (~((~0U) << (size))) << start_bit;
}

}  // namespace

#define MIN(a, b) (a) < (b) ? (a) : (b)
template <typename T>
bool ExtractUint(const uint8_t* report, size_t report_len, const hid::Attributes& attr,
                 T* value_out) {
  static_assert(std::is_pod<T>::value, "not POD");
  if (attr.bit_sz > sizeof(T) * 8) {
    return false;
  }
  if (!FieldFits(report_len, attr)) {
    return false;
  }
  T val = 0;

  const uint32_t start_bit = attr.offset;
  const uint32_t end_bit = start_bit + attr.bit_sz;
  uint32_t index_bit = attr.offset;
  while (index_bit < end_bit) {
    uint8_t bits_till_byte_end = static_cast<uint8_t>(8u - (index_bit % 8u));
    uint8_t bit_count = static_cast<uint8_t>(MIN(bits_till_byte_end, end_bit - index_bit));

    uint8_t extracted_bits = ExtractBitsFromByte(report[index_bit / 8u], index_bit % 8u, bit_count);

    val = static_cast<T>(val | (extracted_bits << (index_bit - start_bit)));

    index_bit += bit_count;
  }

  *value_out = val;
  return true;
}

bool ExtractUint(const uint8_t* report, size_t report_len, const hid::Attributes& attr,
                 uint8_t* value_out) {
  return ExtractUint<uint8_t>(report, report_len, attr, value_out);
}

bool ExtractUint(const uint8_t* report, size_t report_len, const hid::Attributes& attr,
                 uint16_t* value_out) {
  return ExtractUint<uint16_t>(report, report_len, attr, value_out);
}

bool ExtractUint(const uint8_t* report, size_t report_len, const hid::Attributes& attr,
                 uint32_t* value_out) {
  return ExtractUint<uint32_t>(report, report_len, attr, value_out);
}

bool ExtractAsUnit(const uint8_t* report, size_t report_len, const hid::Attributes& attr,
                   double* value_out) {
  if (value_out == nullptr) {
    return false;
  }

  uint32_t uint_out;
  bool ret = ExtractUint(report, report_len, attr, &uint_out);
  if (!ret) {
    return false;
  }

  // If the minimum value is less than zero, then the maximum
  // value and the value itself are an unsigned number. Otherwise they
  // are signed numbers.
  const int64_t logc_max =
      (attr.logc_mm.min < 0) ? attr.logc_mm.max : static_cast<uint32_t>(attr.logc_mm.max);
  int64_t phys_max =
      (attr.phys_mm.min < 0) ? attr.phys_mm.max : static_cast<uint32_t>(attr.phys_mm.max);
  double val = (attr.logc_mm.min < 0)
                   ? static_cast<double>(SignExtendFromNBits(uint_out, attr.bit_sz))
                   : uint_out;

  if (val < static_cast<double>(attr.logc_mm.min) || val > static_cast<double>(attr.logc_mm.max)) {
    return false;
  }

  int64_t phys_min = attr.phys_mm.min;
  if (phys_max == 0 && phys_min == 0) {
    phys_min = attr.logc_mm.min;
    phys_max = logc_max;
  }

  double resolution =
      static_cast<double>(logc_max - attr.logc_mm.min) / static_cast<double>(phys_max - phys_min);
  *value_out = val / resolution;

  return true;
}

bool ExtractWithUnit(const uint8_t* report, size_t report_len, const hid::Attributes& attr,
                     const Unit& unit_out, double* value_out) {
  double val = 0;
  if (!ExtractAsUnit(report, report_len, attr, &val)) {
    return false;
  }

  return unit::ConvertUnits(attr.unit, val, unit_out, value_out);
}

bool InsertUint(uint8_t* report, size_t report_len, const hid::Attributes& attr,
                uint32_t value_in) {
  if (attr.bit_sz > sizeof(int32_t) * 8) {
    return false;
  }
  if (!(FieldFits(report_len, attr))) {
    return false;
  }

  const uint32_t start_bit = attr.offset;
  const uint32_t end_bit = start_bit + attr.bit_sz;
  uint32_t index_bit = attr.offset;
  // Fill in the data from index bit to end bit, going at most a full byte at
  // a time. For the first or the last byte there could be less than a full
  // byte.
  while (index_bit < end_bit) {
    uint8_t bits_from_byte_start = index_bit % 8;
    uint8_t bits_till_byte_end = static_cast<uint8_t>(8u - (bits_from_byte_start));
    uint8_t bit_count = static_cast<uint8_t>(MIN(bits_till_byte_end, end_bit - index_bit));

    // Get the bits from value_in.
    uint32_t value_in_start_bit = index_bit - start_bit;
    uint32_t value_in_bits =
        (value_in & create_mask(value_in_start_bit, bit_count)) >> value_in_start_bit;
    uint8_t value = static_cast<uint8_t>(value_in_bits << bits_from_byte_start);

    // Get the bits from the report data.
    uint8_t data_mask = static_cast<uint8_t>(~create_mask(bits_from_byte_start, bit_count));
    value |= report[index_bit / 8] & data_mask;

    report[index_bit / 8] = value;

    index_bit += bit_count;
  }

  return true;
}

bool InsertAsUnit(uint8_t* report, size_t report_len, const hid::Attributes& attr,
                  double value_in) {
  // If the minimum value is less than zero, then the maximum
  // value and the value itself are an unsigned number. Otherwise they
  // are signed numbers.
  const int64_t logc_max =
      (attr.logc_mm.min < 0) ? attr.logc_mm.max : static_cast<uint32_t>(attr.logc_mm.max);
  int64_t phys_max =
      (attr.phys_mm.min < 0) ? attr.phys_mm.max : static_cast<uint32_t>(attr.phys_mm.max);
  int64_t phys_min = attr.phys_mm.min;

  if (phys_max == 0 && phys_min == 0) {
    phys_min = attr.logc_mm.min;
    phys_max = logc_max;
  }

  if (value_in < static_cast<double>(phys_min) || value_in > static_cast<double>(phys_max)) {
    return false;
  }

  double resolution =
      static_cast<double>(logc_max - attr.logc_mm.min) / static_cast<double>(phys_max - phys_min);
  value_in = value_in * resolution;

  // Do a conversion to int32_t and reinterpret it into a uint32_t while keeping all bits the
  // same. Without this some negative numbers get converted to 0.
  int32_t signed_value_in = static_cast<int32_t>(value_in);
  uint32_t value_in_bytes = *reinterpret_cast<uint32_t*>(&signed_value_in);

  return InsertUint(report, report_len, attr, value_in_bytes);
}

bool InsertWithUnit(uint8_t* report, size_t report_len, const hid::Attributes& attr,
                    const Unit& unit_in, double value_in) {
  double value_converted;
  if (!unit::ConvertUnits(unit_in, value_in, attr.unit, &value_converted)) {
    return false;
  }

  return InsertAsUnit(report, report_len, attr, value_converted);
}

bool InsertAsUnitType(uint8_t* report, size_t report_len, const hid::Attributes& attr,
                      double value_in) {
  Unit unit_in = unit::GetUnitFromUnitType(unit::GetUnitTypeFromUnit(attr.unit));
  double value_out;
  if (!unit::ConvertUnits(unit_in, value_in, attr.unit, &value_out)) {
    return false;
  }

  return InsertAsUnit(report, report_len, attr, value_out);
}

bool ExtractAsUnitType(const uint8_t* report, size_t report_len, const hid::Attributes& attr,
                       double* value_out) {
  double val_out;
  bool ret = ExtractAsUnit(report, report_len, attr, &val_out);
  if (!ret) {
    return false;
  }

  *value_out = unit::ConvertValToUnitType(attr.unit, val_out);
  return true;
}

#undef MIN
}  // namespace hid