Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 0bfbee16 authored by Chienyuan Huang's avatar Chienyuan Huang
Browse files

CS: Parsing the CS procedure data of mode 1

Flag: com.android.bluetooth.flags.channel_sounding_in_stack
Bug: 324185011
Bug: 376172766
Test: m com.android.btservice
Change-Id: I0c0d8a9b7a536647ce588934fdbcf73c73e4b0b6
parent cdd84ade
Loading
Loading
Loading
Loading
+4 −0
Original line number Diff line number Diff line
@@ -35,6 +35,10 @@ struct ChannelSoundingRawData {
  std::vector<std::vector<std::complex<double>>> tone_pct_reflector_;
  std::vector<std::vector<uint8_t>> tone_quality_indicator_initiator_;
  std::vector<std::vector<uint8_t>> tone_quality_indicator_reflector_;
  std::vector<int8_t> packet_quality_initiator;
  std::vector<int8_t> packet_quality_reflector;
  std::vector<int16_t> toa_tod_initiators_;
  std::vector<int16_t> tod_toa_reflectors_;
};

struct RangingResult {
+14 −0
Original line number Diff line number Diff line
@@ -195,6 +195,7 @@ public:
    hal_raw_data.stepChannels = raw_data.step_channel_;
    hal_raw_data.initiatorData.stepTonePcts.emplace(std::vector<std::optional<StepTonePct>>{});
    hal_raw_data.reflectorData.stepTonePcts.emplace(std::vector<std::optional<StepTonePct>>{});
    // Add tone data for mode 2
    for (uint8_t i = 0; i < raw_data.tone_pct_initiator_.size(); i++) {
      StepTonePct step_tone_pct;
      for (uint8_t j = 0; j < raw_data.tone_pct_initiator_[i].size(); j++) {
@@ -217,6 +218,19 @@ public:
      step_tone_pct.toneQualityIndicator = raw_data.tone_quality_indicator_reflector_[i];
      hal_raw_data.reflectorData.stepTonePcts.value().emplace_back(step_tone_pct);
    }
    // Add RTT data for mode 1
    if (!raw_data.toa_tod_initiators_.empty()) {
      hal_raw_data.toaTodInitiator = std::vector<int32_t>(raw_data.toa_tod_initiators_.begin(),
                                                          raw_data.toa_tod_initiators_.end());
      hal_raw_data.initiatorData.packetQuality = std::vector<uint8_t>(
              raw_data.packet_quality_initiator.begin(), raw_data.packet_quality_initiator.end());
    }
    if (!raw_data.tod_toa_reflectors_.empty()) {
      hal_raw_data.todToaReflector = std::vector<int32_t>(raw_data.tod_toa_reflectors_.begin(),
                                                          raw_data.tod_toa_reflectors_.end());
      hal_raw_data.reflectorData.packetQuality = std::vector<uint8_t>(
              raw_data.packet_quality_reflector.begin(), raw_data.packet_quality_reflector.end());
    }
    session_trackers_[connection_handle]->GetSession()->writeRawData(hal_raw_data);
  }

+133 −1
Original line number Diff line number Diff line
@@ -119,6 +119,14 @@ struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
    std::vector<std::vector<std::complex<double>>> tone_pct_reflector;
    std::vector<std::vector<uint8_t>> tone_quality_indicator_initiator;
    std::vector<std::vector<uint8_t>> tone_quality_indicator_reflector;
    std::vector<int8_t> packet_quality_initiator;
    std::vector<int8_t> packet_quality_reflector;
    std::vector<int16_t> toa_tod_initiators;
    std::vector<int16_t> tod_toa_reflectors;
    std::vector<int8_t> rssi_initiator;
    std::vector<int8_t> rssi_reflector;
    bool contains_sounding_sequence_local_;
    bool contains_sounding_sequence_remote_;
    CsProcedureDoneStatus local_status;
    CsProcedureDoneStatus remote_status;
    // If the procedure is aborted by either the local or remote side.
@@ -742,6 +750,7 @@ struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
    }
    cs_subfeature_supported_ = complete_view.GetOptionalSubfeaturesSupported();
    num_antennas_supported_ = complete_view.GetNumAntennasSupported();
    local_support_phase_based_ranging_ = cs_subfeature_supported_.phase_based_ranging_ == 0x01;
  }

  void on_cs_read_remote_supported_capabilities_complete(
@@ -1334,6 +1343,57 @@ struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
            }
            parse_index = after;
          } break;
          case 1: {
            if (remote_role == CsRole::INITIATOR) {
              if (procedure_data->contains_sounding_sequence_remote_) {
                LeCsMode1InitatorDataWithPacketPct tone_data;
                after = LeCsMode1InitatorDataWithPacketPct::Parse(&tone_data, parse_index);
                if (after == parse_index) {
                  log::warn("Error invalid mode {} data, role:{}", step_mode.mode_type_,
                            CsRoleText(remote_role));
                  return;
                }
                parse_index = after;
                procedure_data->toa_tod_initiators.emplace_back(tone_data.toa_tod_initiator_);
                procedure_data->packet_quality_initiator.emplace_back(tone_data.packet_quality_);
              } else {
                LeCsMode1InitatorData tone_data;
                after = LeCsMode1InitatorData::Parse(&tone_data, parse_index);
                if (after == parse_index) {
                  log::warn("Error invalid mode {} data, role:{}", step_mode.mode_type_,
                            CsRoleText(remote_role));
                  return;
                }
                parse_index = after;
                procedure_data->toa_tod_initiators.emplace_back(tone_data.toa_tod_initiator_);
                procedure_data->packet_quality_initiator.emplace_back(tone_data.packet_quality_);
              }
            } else {
              if (procedure_data->contains_sounding_sequence_remote_) {
                LeCsMode1ReflectorDataWithPacketPct tone_data;
                after = LeCsMode1ReflectorDataWithPacketPct::Parse(&tone_data, parse_index);
                if (after == parse_index) {
                  log::warn("Error invalid mode {} data, role:{}", step_mode.mode_type_,
                            CsRoleText(remote_role));
                  return;
                }
                parse_index = after;
                procedure_data->tod_toa_reflectors.emplace_back(tone_data.tod_toa_reflector_);
                procedure_data->packet_quality_reflector.emplace_back(tone_data.packet_quality_);
              } else {
                LeCsMode1ReflectorData tone_data;
                after = LeCsMode1ReflectorData::Parse(&tone_data, parse_index);
                if (after == parse_index) {
                  log::warn("Error invalid mode {} data, role:{}", step_mode.mode_type_,
                            CsRoleText(remote_role));
                  return;
                }
                parse_index = after;
                procedure_data->tod_toa_reflectors.emplace_back(tone_data.tod_toa_reflector_);
                procedure_data->packet_quality_reflector.emplace_back(tone_data.packet_quality_);
              }
            }
          } break;
          case 2: {
            uint8_t num_tone_data = num_antenna_paths + 1;
            uint8_t data_len = 1 + (4 * num_tone_data);
@@ -1415,6 +1475,18 @@ struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
    data_list.emplace_back(procedure_counter, num_antenna_paths, live_tracker->config_id,
                           live_tracker->selected_tx_power);

    // Check if sounding phase-based ranging is supported, and RTT type contains a sounding
    // sequence
    bool rtt_contains_sounding_sequence = false;
    if (live_tracker->rtt_type == CsRttType::RTT_WITH_32_BIT_SOUNDING_SEQUENCE ||
        live_tracker->rtt_type == CsRttType::RTT_WITH_96_BIT_SOUNDING_SEQUENCE) {
      rtt_contains_sounding_sequence = true;
    }
    data_list.back().contains_sounding_sequence_local_ =
            local_support_phase_based_ranging_ && rtt_contains_sounding_sequence;
    data_list.back().contains_sounding_sequence_remote_ =
            live_tracker->remote_support_phase_based_ranging && rtt_contains_sounding_sequence;

    // Append ranging header raw data
    std::vector<uint8_t> ranging_header_raw = {};
    BitInserter bi(ranging_header_raw);
@@ -1482,6 +1554,10 @@ struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
        raw_data.tone_pct_reflector_ = procedure_data->tone_pct_reflector;
        raw_data.tone_quality_indicator_reflector_ =
                procedure_data->tone_quality_indicator_reflector;
        raw_data.toa_tod_initiators_ = procedure_data->toa_tod_initiators;
        raw_data.tod_toa_reflectors_ = procedure_data->tod_toa_reflectors;
        raw_data.packet_quality_initiator = procedure_data->packet_quality_initiator;
        raw_data.packet_quality_reflector = procedure_data->packet_quality_reflector;
        ranging_hal_->WriteRawData(connection_handle, raw_data);
        return;
      }
@@ -1552,6 +1628,62 @@ struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
            log::verbose("step_data: {}", tone_data_view.ToString());
          }
        } break;
        case 1: {
          if (role == CsRole::INITIATOR) {
            if (procedure_data.contains_sounding_sequence_local_) {
              LeCsMode1InitatorDataWithPacketPct tone_data_view;
              auto after = LeCsMode1InitatorDataWithPacketPct::Parse(&tone_data_view, iterator);
              if (after == iterator) {
                log::warn("Received invalid mode {} data, role:{}", mode, CsRoleText(role));
                print_raw_data(result_data_structure.step_data_);
                continue;
              }
              log::verbose("step_data: {}", tone_data_view.ToString());
              procedure_data.rssi_initiator.emplace_back(tone_data_view.packet_rssi_);
              procedure_data.toa_tod_initiators.emplace_back(tone_data_view.toa_tod_initiator_);
              procedure_data.packet_quality_initiator.emplace_back(tone_data_view.packet_quality_);
            } else {
              LeCsMode1InitatorData tone_data_view;
              auto after = LeCsMode1InitatorData::Parse(&tone_data_view, iterator);
              if (after == iterator) {
                log::warn("Received invalid mode {} data, role:{}", mode, CsRoleText(role));
                print_raw_data(result_data_structure.step_data_);
                continue;
              }
              log::verbose("step_data: {}", tone_data_view.ToString());
              procedure_data.rssi_initiator.emplace_back(tone_data_view.packet_rssi_);
              procedure_data.toa_tod_initiators.emplace_back(tone_data_view.toa_tod_initiator_);
              procedure_data.packet_quality_initiator.emplace_back(tone_data_view.packet_quality_);
            }
            procedure_data.step_channel.push_back(step_channel);
          } else {
            if (procedure_data.contains_sounding_sequence_local_) {
              LeCsMode1ReflectorDataWithPacketPct tone_data_view;
              auto after = LeCsMode1ReflectorDataWithPacketPct::Parse(&tone_data_view, iterator);
              if (after == iterator) {
                log::warn("Received invalid mode {} data, role:{}", mode, CsRoleText(role));
                print_raw_data(result_data_structure.step_data_);
                continue;
              }
              log::verbose("step_data: {}", tone_data_view.ToString());
              procedure_data.rssi_reflector.emplace_back(tone_data_view.packet_rssi_);
              procedure_data.tod_toa_reflectors.emplace_back(tone_data_view.tod_toa_reflector_);
              procedure_data.packet_quality_reflector.emplace_back(tone_data_view.packet_quality_);
            } else {
              LeCsMode1ReflectorData tone_data_view;
              auto after = LeCsMode1ReflectorData::Parse(&tone_data_view, iterator);
              if (after == iterator) {
                log::warn("Received invalid mode {} data, role:{}", mode, CsRoleText(role));
                print_raw_data(result_data_structure.step_data_);
                continue;
              }
              log::verbose("step_data: {}", tone_data_view.ToString());
              procedure_data.rssi_reflector.emplace_back(tone_data_view.packet_rssi_);
              procedure_data.tod_toa_reflectors.emplace_back(tone_data_view.tod_toa_reflector_);
              procedure_data.packet_quality_reflector.emplace_back(tone_data_view.packet_quality_);
            }
          }
        } break;
        case 2: {
          LeCsMode2Data tone_data_view;
          auto after = LeCsMode2Data::Parse(&tone_data_view, iterator);
@@ -1589,7 +1721,6 @@ struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
            }
          }
        } break;
        case 1:
        case 3:
          log::debug("Unsupported mode: {}", mode);
          break;
@@ -1780,6 +1911,7 @@ struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
  DistanceMeasurementCallbacks* distance_measurement_callbacks_;
  CsOptionalSubfeaturesSupported cs_subfeature_supported_;
  uint8_t num_antennas_supported_ = 0x01;
  bool local_support_phase_based_ranging_ = false;
  // A table that maps num_antennas_supported and remote_num_antennas_supported to Antenna
  // Configuration Index.
  uint8_t cs_tone_antenna_config_mapping_table_[4][4] = {
+52 −0
Original line number Diff line number Diff line
@@ -6895,6 +6895,58 @@ struct LeCsMode0ReflectorData {
  packet_antenna : 8,
}

enum CsPacketNadm : 8 {
  ATTACK_IS_EXTREMELY_UNLIKELY = 0x00,
  ATTACK_IS_VERY_UNLIKELY = 0x01,
  ATTACK_IS_UNLIKELY = 0x02,
  ATTACK_IS_POSSIBLE = 0x03,
  ATTACK_IS_LIKELY = 0x04,
  ATTACK_IS_VERY_LIKELY = 0x05,
  ATTACK_IS_EXTREMELY_LIKELY = 0x06,
  UNKNOWN_NADM = 0xFF,
}

struct LeCsToneData {
  i_sample : 12,
  q_sample : 12,
}

struct LeCsMode1InitatorData {
  packet_quality : 8,
  packet_nadm : CsPacketNadm,
  packet_rssi : 8,
  toa_tod_initiator : 16,
  packet_antenna : 8,
}

struct LeCsMode1InitatorDataWithPacketPct {
  packet_quality : 8,
  packet_nadm : CsPacketNadm,
  packet_rssi : 8,
  toa_tod_initiator : 16,
  packet_antenna : 8,
  packet_pct1 : LeCsToneData,
  packet_pct2 : LeCsToneData,
}

struct LeCsMode1ReflectorData {
  packet_quality : 8,
  packet_nadm : CsPacketNadm,
  packet_rssi : 8,
  tod_toa_reflector : 16,
  packet_antenna : 8,
}

struct LeCsMode1ReflectorDataWithPacketPct {
  packet_quality : 8,
  packet_nadm : CsPacketNadm,
  packet_rssi : 8,
  tod_toa_reflector : 16,
  packet_antenna : 8,
  packet_pct1 : LeCsToneData,
  packet_pct2 : LeCsToneData,
}

struct LeCsToneDataWithQuality {
  i_sample : 12,
  q_sample : 12,