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

system/gd/hal/ranging_hal.h

+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 {

system/gd/hal/ranging_hal_android.cc

+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);

  }

system/gd/hci/distance_measurement_manager.cc

+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] = {

system/pdl/hci/hci_packets.pdl

+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,