1 // Copyright 2022 Google LLC 2 // 3 // Licensed under the Apache License, Version 2.0 (the "License"); 4 // you may not use this file except in compliance with the License. 5 // You may obtain a copy of the License at 6 // 7 // https://www.apache.org/licenses/LICENSE-2.0 8 // 9 // Unless required by applicable law or agreed to in writing, software 10 // distributed under the License is distributed on an "AS IS" BASIS, 11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 // See the License for the specific language governing permissions and 13 // limitations under the License. 14 15 use crate::packets::uci::{self, *}; 16 use crate::MacAddress; 17 use crate::PicaCommand; 18 19 use std::collections::HashMap; 20 use std::time::Duration; 21 22 use pdl_runtime::Packet; 23 use tokio::sync::mpsc; 24 use tokio::time; 25 26 use super::app_config::SubSessionKey; 27 use super::session::Session; 28 use super::UciPacket; 29 30 pub const MAX_DEVICE: usize = 4; 31 pub const MAX_SESSION: usize = 255; 32 33 const UCI_VERSION: u16 = 0x0002; // Version 2.0 34 const MAC_VERSION: u16 = 0x3001; // Version 1.3.0 35 const PHY_VERSION: u16 = 0x3001; // Version 1.3.0 36 const TEST_VERSION: u16 = 0x1001; // Version 1.1 37 38 /// cf. [UCI] 8.3 Table 29 39 pub const MAX_NUMBER_OF_CONTROLEES: usize = 8; 40 41 // Capabilities are vendor defined 42 // Android compliant: FIRA-287 UCI_Generic_Specification controlee capabilities_r4 43 // Android parses capabilities, according to these definitions: 44 // /android/packages/modules/Uwb/service/java/com/android/server/uwb/config/CapabilityParam.java 45 pub const DEFAULT_CAPS_INFO: &[(CapTlvType, &[u8])] = &[ 46 // Fira params 47 (CapTlvType::SupportedFiraPhyVersionRange, &[1, 1, 1, 3]), // 1.1 - 1.3 48 (CapTlvType::SupportedFiraMacVersionRange, &[1, 1, 1, 3]), // 1.1 - 1.3 49 (CapTlvType::SupportedDeviceRoles, &[0x3]), // INTIATOR | RESPONDER 50 (CapTlvType::SupportedRangingMethod, &[0x1f]), // DS_TWR_NON_DEFERRED | SS_TWR_NON_DEFERRED | DS_TWR_DEFERRED | SS_TWR_DEFERRED | OWR 51 (CapTlvType::SupportedStsConfig, &[0x1f]), // STATIC_STS | DYNAMIC_STS | DYNAMIC_STS_RESPONDER_SPECIFIC_SUBSESSION_KEY | PROVISIONED_STS | PROVISIONED_STS_RESPONDER_SPECIFIC_SUBSESSION_KEY 52 (CapTlvType::SupportedMultiNodeModes, &[0xff]), 53 (CapTlvType::SupportedRangingTimeStruct, &[0x01]), // Block Based Scheduling (default) 54 (CapTlvType::SupportedScheduledMode, &[0x01]), // Time scheduled ranging (default) 55 (CapTlvType::SupportedHoppingMode, &[0x00]), // Hopping disable 56 (CapTlvType::SupportedBlockStriding, &[0x1]), 57 (CapTlvType::SupportedUwbInitiationTime, &[0x01]), 58 (CapTlvType::SupportedChannels, &[0xff]), 59 (CapTlvType::SupportedRframeConfig, &[0xff]), 60 (CapTlvType::SupportedCcConstraintLength, &[0xff]), 61 (CapTlvType::SupportedBprfParameterSets, &[0xff]), 62 (CapTlvType::SupportedHprfParameterSets, &[0xff]), 63 (CapTlvType::SupportedAoa, &[0xff]), 64 (CapTlvType::SupportedAoaResultReqAntennaInterleaving, &[0x1]), 65 (CapTlvType::SupportedExtendedMacAddress, &[0x1]), 66 // CCC params 67 (CapTlvType::CccSupportedVersions, &[1, 0]), 68 (CapTlvType::CccSupportedUwbConfigs, &[0]), 69 (CapTlvType::CccSupportedPulseShapeCombos, &[0]), 70 (CapTlvType::CccSupportedRanMultiplier, &[0, 0, 0, 0]), 71 (CapTlvType::CccSupportedChapsPerSlot, &[0xff]), 72 (CapTlvType::CccSupportedSyncCodes, &[0xff, 0xff, 0xff, 0xff]), 73 (CapTlvType::CccSupportedChannels, &[0xff]), 74 ( 75 CapTlvType::CccSupportedHoppingConfigModesAndSequences, 76 &[0xff], 77 ), 78 ]; 79 80 /// [UCI] 8.2 Device Configuration Parameters 81 pub struct DeviceConfig { 82 device_state: DeviceState, 83 // This config is used to enable/disable the low power mode. 84 // 0x00 = Disable low power mode 85 // 0x01 = Enable low power mode (default) 86 low_power_mode: bool, 87 } 88 89 // [UCI] 6.3.1 Setting the Configuration 90 // All device configuration parameters within the UWBS are set to 91 // default values at Table 44 [..]. 92 impl Default for DeviceConfig { default() -> Self93 fn default() -> Self { 94 DeviceConfig { 95 device_state: DeviceState::DeviceStateError, 96 low_power_mode: true, 97 } 98 } 99 } 100 101 pub struct Device { 102 pub handle: usize, 103 pub mac_address: MacAddress, 104 config: DeviceConfig, 105 /// [UCI] 5. UWBS Device State Machine 106 state: DeviceState, 107 sessions: HashMap<u32, Session>, 108 pub tx: mpsc::UnboundedSender<UciPacket>, 109 pica_tx: mpsc::Sender<PicaCommand>, 110 country_code: [u8; 2], 111 112 pub n_active_sessions: usize, 113 } 114 115 impl Device { new( handle: usize, mac_address: MacAddress, tx: mpsc::UnboundedSender<UciPacket>, pica_tx: mpsc::Sender<PicaCommand>, ) -> Self116 pub fn new( 117 handle: usize, 118 mac_address: MacAddress, 119 tx: mpsc::UnboundedSender<UciPacket>, 120 pica_tx: mpsc::Sender<PicaCommand>, 121 ) -> Self { 122 Device { 123 handle, 124 mac_address, 125 config: Default::default(), 126 state: DeviceState::DeviceStateError, // Will be overwitten 127 sessions: Default::default(), 128 tx, 129 pica_tx, 130 country_code: Default::default(), 131 n_active_sessions: 0, 132 } 133 } 134 set_state(&mut self, device_state: DeviceState)135 pub fn set_state(&mut self, device_state: DeviceState) { 136 // No transition: ignore 137 if device_state == self.state { 138 return; 139 } 140 141 // Send status notification 142 self.state = device_state; 143 let tx = self.tx.clone(); 144 tokio::spawn(async move { 145 time::sleep(Duration::from_millis(5)).await; 146 tx.send( 147 CoreDeviceStatusNtfBuilder { device_state } 148 .build() 149 .encode_to_vec() 150 .unwrap(), 151 ) 152 .unwrap() 153 }); 154 } 155 init(&mut self)156 pub fn init(&mut self) { 157 self.set_state(DeviceState::DeviceStateReady); 158 } 159 session(&self, session_id: u32) -> Option<&Session>160 pub fn session(&self, session_id: u32) -> Option<&Session> { 161 self.sessions.get(&session_id) 162 } 163 session_mut(&mut self, session_id: u32) -> Option<&mut Session>164 pub fn session_mut(&mut self, session_id: u32) -> Option<&mut Session> { 165 self.sessions.get_mut(&session_id) 166 } 167 can_start_ranging(&self, peer_session: &Session, session_id: u32) -> bool168 pub fn can_start_ranging(&self, peer_session: &Session, session_id: u32) -> bool { 169 match self.session(session_id) { 170 Some(session) => { 171 session.session_state() == SessionState::SessionStateActive 172 && session 173 .app_config 174 .is_compatible_for_ranging(&peer_session.app_config) 175 } 176 None => false, 177 } 178 } 179 can_start_data_transfer(&self, session_id: u32) -> bool180 pub fn can_start_data_transfer(&self, session_id: u32) -> bool { 181 match self.session(session_id) { 182 Some(session) => { 183 session.session_state() == SessionState::SessionStateActive 184 && session.session_type() == SessionType::FiraRangingAndInBandDataSession 185 && session.app_config.can_start_data_transfer() 186 } 187 None => false, 188 } 189 } 190 can_receive_data_transfer(&self, session_id: u32) -> bool191 pub fn can_receive_data_transfer(&self, session_id: u32) -> bool { 192 match self.session(session_id) { 193 Some(session) => { 194 session.session_state() == SessionState::SessionStateActive 195 && session.session_type() == SessionType::FiraRangingAndInBandDataSession 196 && session.app_config.can_receive_data_transfer() 197 } 198 None => false, 199 } 200 } 201 202 // Send a response or notification to the Host. send_raw_control(&mut self, packet: Vec<u8>)203 fn send_raw_control(&mut self, packet: Vec<u8>) { 204 let _ = self.tx.send(packet); 205 } 206 207 // Send a response or notification to the Host. send_control(&mut self, packet: impl Packet)208 fn send_control(&mut self, packet: impl Packet) { 209 self.send_raw_control(packet.encode_to_vec().unwrap()); 210 } 211 212 // The fira norm specify to send a response, then reset, then 213 // send a notification once the reset is done core_device_reset(&mut self, cmd: CoreDeviceResetCmd) -> CoreDeviceResetRsp214 fn core_device_reset(&mut self, cmd: CoreDeviceResetCmd) -> CoreDeviceResetRsp { 215 let reset_config = cmd.get_reset_config(); 216 log::debug!("[{}] DeviceReset", self.handle); 217 log::debug!(" reset_config={:?}", reset_config); 218 219 let status = match reset_config { 220 ResetConfig::UwbsReset => uci::Status::Ok, 221 }; 222 *self = Device::new( 223 self.handle, 224 self.mac_address, 225 self.tx.clone(), 226 self.pica_tx.clone(), 227 ); 228 self.init(); 229 230 CoreDeviceResetRspBuilder { status }.build() 231 } 232 core_get_device_info(&self, _cmd: CoreGetDeviceInfoCmd) -> CoreGetDeviceInfoRsp233 fn core_get_device_info(&self, _cmd: CoreGetDeviceInfoCmd) -> CoreGetDeviceInfoRsp { 234 // TODO: Implement a fancy build time state machine instead of crash at runtime 235 log::debug!("[{}] GetDeviceInfo", self.handle); 236 assert_eq!(self.state, DeviceState::DeviceStateReady); 237 CoreGetDeviceInfoRspBuilder { 238 status: uci::Status::Ok, 239 uci_version: UCI_VERSION, 240 mac_version: MAC_VERSION, 241 phy_version: PHY_VERSION, 242 uci_test_version: TEST_VERSION, 243 vendor_spec_info: Vec::new(), 244 } 245 .build() 246 } 247 core_get_caps_info(&self, _cmd: CoreGetCapsInfoCmd) -> CoreGetCapsInfoRsp248 pub fn core_get_caps_info(&self, _cmd: CoreGetCapsInfoCmd) -> CoreGetCapsInfoRsp { 249 log::debug!("[{}] GetCapsInfo", self.handle); 250 251 let caps = DEFAULT_CAPS_INFO 252 .iter() 253 .map(|(id, value)| CapTlv { 254 t: *id, 255 v: (*value).into(), 256 }) 257 .collect(); 258 259 CoreGetCapsInfoRspBuilder { 260 status: uci::Status::Ok, 261 tlvs: caps, 262 } 263 .build() 264 } 265 core_set_config(&mut self, cmd: CoreSetConfigCmd) -> CoreSetConfigRsp266 pub fn core_set_config(&mut self, cmd: CoreSetConfigCmd) -> CoreSetConfigRsp { 267 log::debug!("[{}] SetConfig", self.handle); 268 assert_eq!(self.state, DeviceState::DeviceStateReady); // UCI 6.3 269 270 // [UCI] 6.3.1 Setting the Configuration 271 // The UWBS shall respond with CORE_SET_CONFIG_RSP setting the Status 272 // field of STATUS_INVALID_PARAM and including one or more invalid 273 // Parameter ID(s) If the Host tries to set a parameter that is not 274 // available in the UWBS. All other configuration parameters should 275 // have been set to the new values within the UWBS. 276 let mut invalid_parameters = vec![]; 277 for parameter in cmd.get_parameters() { 278 match parameter.id { 279 uci::ConfigParameterId::DeviceState => { 280 invalid_parameters.push(uci::ConfigParameterStatus { 281 id: parameter.id, 282 status: uci::Status::ReadOnly, 283 }) 284 } 285 uci::ConfigParameterId::LowPowerMode => { 286 self.config.low_power_mode = parameter.value.first().copied().unwrap_or(1) != 0; 287 } 288 uci::ConfigParameterId::Rfu(id) => { 289 log::warn!("unknown config parameter id 0x{:02x}", *id); 290 invalid_parameters.push(uci::ConfigParameterStatus { 291 id: parameter.id, 292 status: uci::Status::InvalidParam, 293 }) 294 } 295 } 296 } 297 298 CoreSetConfigRspBuilder { 299 status: if invalid_parameters.is_empty() { 300 uci::Status::Ok 301 } else { 302 uci::Status::InvalidParam 303 }, 304 parameters: invalid_parameters, 305 } 306 .build() 307 } 308 core_get_config(&self, cmd: CoreGetConfigCmd) -> CoreGetConfigRsp309 pub fn core_get_config(&self, cmd: CoreGetConfigCmd) -> CoreGetConfigRsp { 310 log::debug!("[{}] GetConfig", self.handle); 311 312 // [UCI] 6.3.2 Retrieve the Configuration 313 // If the Host tries to retrieve any Parameter(s) that are not available 314 // in the UWBS, the UWBS shall respond with a CORE_GET_CONFIG_RSP with 315 // a Status field of STATUS_INVALID_PARAM, containing each unavailable 316 // Device Configuration Parameter Type with Length field is zero. In 317 // this case, the CORE_GET_CONFIG_RSP shall not include any parameter(s) 318 // that are available in the UWBS. 319 let mut valid_parameters = vec![]; 320 let mut invalid_parameters = vec![]; 321 for id in cmd.get_parameter_ids() { 322 match id { 323 ConfigParameterId::DeviceState => valid_parameters.push(ConfigParameter { 324 id: *id, 325 value: vec![self.config.device_state.into()], 326 }), 327 ConfigParameterId::LowPowerMode => valid_parameters.push(ConfigParameter { 328 id: *id, 329 value: vec![self.config.low_power_mode.into()], 330 }), 331 ConfigParameterId::Rfu(_) => invalid_parameters.push(ConfigParameter { 332 id: *id, 333 value: vec![], 334 }), 335 } 336 } 337 338 if invalid_parameters.is_empty() { 339 CoreGetConfigRspBuilder { 340 status: uci::Status::Ok, 341 parameters: valid_parameters, 342 } 343 .build() 344 } else { 345 CoreGetConfigRspBuilder { 346 status: uci::Status::InvalidParam, 347 parameters: invalid_parameters, 348 } 349 .build() 350 } 351 } 352 session_init(&mut self, cmd: SessionInitCmd) -> SessionInitRsp353 fn session_init(&mut self, cmd: SessionInitCmd) -> SessionInitRsp { 354 let session_id = cmd.get_session_id(); 355 let session_type = cmd.get_session_type(); 356 357 log::debug!("[{}] Session init", self.handle); 358 log::debug!(" session_id=0x{:x}", session_id); 359 log::debug!(" session_type={:?}", session_type); 360 361 let status = if self.sessions.len() >= MAX_SESSION { 362 uci::Status::ErrorMaxSessionsExceeded 363 } else { 364 match self.sessions.insert( 365 session_id, 366 Session::new(session_id, session_type, self.handle, self.tx.clone()), 367 ) { 368 Some(_) => uci::Status::ErrorSessionDuplicate, 369 None => { 370 // Should not fail 371 self.session_mut(session_id).unwrap().init(); 372 uci::Status::Ok 373 } 374 } 375 }; 376 377 SessionInitRspBuilder { status }.build() 378 } 379 session_deinit(&mut self, cmd: SessionDeinitCmd) -> SessionDeinitRsp380 fn session_deinit(&mut self, cmd: SessionDeinitCmd) -> SessionDeinitRsp { 381 let session_id = cmd.get_session_token(); 382 log::debug!("[{}] Session deinit", self.handle); 383 log::debug!(" session_id=0x{:x}", session_id); 384 385 let status = match self.sessions.get_mut(&session_id) { 386 Some(session) => { 387 if session.state == SessionState::SessionStateActive { 388 self.n_active_sessions -= 1; 389 if self.n_active_sessions == 0 { 390 self.set_state(DeviceState::DeviceStateReady); 391 } 392 } 393 self.sessions.remove(&session_id); 394 uci::Status::Ok 395 } 396 None => uci::Status::ErrorSessionNotExist, 397 }; 398 SessionDeinitRspBuilder { status }.build() 399 } 400 session_get_count(&self, _cmd: SessionGetCountCmd) -> SessionGetCountRsp401 fn session_get_count(&self, _cmd: SessionGetCountCmd) -> SessionGetCountRsp { 402 log::debug!("[{}] Session get count", self.handle); 403 404 SessionGetCountRspBuilder { 405 status: uci::Status::Ok, 406 session_count: self.sessions.len() as u8, 407 } 408 .build() 409 } 410 session_set_app_config(&mut self, cmd: SessionSetAppConfigCmd) -> SessionSetAppConfigRsp411 fn session_set_app_config(&mut self, cmd: SessionSetAppConfigCmd) -> SessionSetAppConfigRsp { 412 let session_handle = cmd.get_session_token(); 413 414 log::debug!( 415 "[{}:0x{:x}] Session Set App Config", 416 self.handle, 417 session_handle 418 ); 419 420 let Some(session) = self.sessions.get_mut(&session_handle) else { 421 return SessionSetAppConfigRspBuilder { 422 cfg_status: Vec::new(), 423 status: uci::Status::ErrorSessionNotExist, 424 } 425 .build(); 426 }; 427 428 assert!( 429 session.session_type == SessionType::FiraRangingSession 430 || session.session_type == SessionType::FiraRangingAndInBandDataSession 431 ); 432 433 if session.state == SessionState::SessionStateActive { 434 const IMMUTABLE_PARAMETERS: &[AppConfigTlvType] = &[AppConfigTlvType::AoaResultReq]; 435 if cmd 436 .get_tlvs() 437 .iter() 438 .any(|cfg| IMMUTABLE_PARAMETERS.contains(&cfg.cfg_id)) 439 { 440 return SessionSetAppConfigRspBuilder { 441 status: uci::Status::ErrorSessionActive, 442 cfg_status: vec![], 443 } 444 .build(); 445 } 446 } 447 448 let (status, invalid_parameters) = if session.state != SessionState::SessionStateInit 449 && session.state != SessionState::SessionStateActive 450 { 451 (uci::Status::Rejected, Vec::new()) 452 } else { 453 let mut app_config = session.app_config.clone(); 454 let mut invalid_parameters = vec![]; 455 for cfg in cmd.get_tlvs() { 456 match app_config.set(cfg.cfg_id, &cfg.v) { 457 Ok(_) => (), 458 Err(_) => invalid_parameters.push(AppConfigStatus { 459 cfg_id: cfg.cfg_id, 460 status: uci::Status::InvalidParam, 461 }), 462 } 463 } 464 465 // [UCI] 7.5.1 Configuration of a Session 466 // This section defines the mandatory APP Configuration Parameters to be applied 467 // by the Host for FiRa defined UWB Session types. The Host shall apply these 468 // mandatory configurations to move the Session State from SESSION_STATE_INIT 469 // to SESSION_STATE_IDLE. 470 // 471 // - DEVICE_ROLE 472 // - MULTI_NODE_MODE 473 // - RANGING_ROUND_USAGE 474 // - DEVICE_MAC_ADDRESS 475 // - DEVICE_TYPE (see Note1) 476 // - SCHEDULE_MODE 477 if app_config.device_role.is_none() 478 || app_config.multi_node_mode.is_none() 479 || app_config.ranging_round_usage.is_none() 480 || app_config.device_mac_address.is_none() 481 || app_config.schedule_mode.is_none() 482 { 483 log::error!( 484 "[{}:0x{:x}] missing mandatory APP config parameters", 485 self.handle, 486 session_handle 487 ); 488 return SessionSetAppConfigRspBuilder { 489 status: uci::Status::Rejected, 490 cfg_status: vec![], 491 } 492 .build(); 493 } 494 495 if invalid_parameters.is_empty() { 496 session.app_config = app_config; 497 if session.state == SessionState::SessionStateInit { 498 session.set_state( 499 SessionState::SessionStateIdle, 500 ReasonCode::StateChangeWithSessionManagementCommands, 501 ); 502 } 503 (uci::Status::Ok, invalid_parameters) 504 } else { 505 (uci::Status::InvalidParam, invalid_parameters) 506 } 507 }; 508 509 SessionSetAppConfigRspBuilder { 510 status, 511 cfg_status: invalid_parameters, 512 } 513 .build() 514 } 515 session_get_app_config(&self, cmd: SessionGetAppConfigCmd) -> SessionGetAppConfigRsp516 fn session_get_app_config(&self, cmd: SessionGetAppConfigCmd) -> SessionGetAppConfigRsp { 517 let session_handle = cmd.get_session_token(); 518 519 log::debug!( 520 "[{}:0x{:x}] Session Get App Config", 521 self.handle, 522 session_handle 523 ); 524 525 let Some(session) = self.sessions.get(&session_handle) else { 526 return SessionGetAppConfigRspBuilder { 527 tlvs: vec![], 528 status: uci::Status::ErrorSessionNotExist, 529 } 530 .build(); 531 }; 532 533 let (status, valid_parameters) = { 534 let mut valid_parameters = vec![]; 535 let mut invalid_parameters = vec![]; 536 for id in cmd.get_app_cfg() { 537 match session.app_config.get(*id) { 538 Ok(value) => valid_parameters.push(AppConfigTlv { 539 cfg_id: *id, 540 v: value, 541 }), 542 Err(_) => invalid_parameters.push(AppConfigTlv { 543 cfg_id: *id, 544 v: vec![], 545 }), 546 } 547 } 548 549 if invalid_parameters.is_empty() { 550 (uci::Status::Ok, valid_parameters) 551 } else { 552 (uci::Status::Failed, Vec::new()) 553 } 554 }; 555 556 SessionGetAppConfigRspBuilder { 557 status, 558 tlvs: valid_parameters, 559 } 560 .build() 561 } 562 session_get_state(&self, cmd: SessionGetStateCmd) -> SessionGetStateRsp563 fn session_get_state(&self, cmd: SessionGetStateCmd) -> SessionGetStateRsp { 564 let session_handle = cmd.get_session_token(); 565 566 log::debug!("[{}:0x{:x}] Session Get State", self.handle, session_handle); 567 568 let Some(session) = self.sessions.get(&session_handle) else { 569 return SessionGetStateRspBuilder { 570 session_state: SessionState::SessionStateInit, 571 status: uci::Status::ErrorSessionNotExist, 572 } 573 .build(); 574 }; 575 576 SessionGetStateRspBuilder { 577 status: uci::Status::Ok, 578 session_state: session.state, 579 } 580 .build() 581 } 582 session_update_controller_multicast_list( &mut self, cmd: SessionUpdateControllerMulticastListCmd, ) -> SessionUpdateControllerMulticastListRsp583 fn session_update_controller_multicast_list( 584 &mut self, 585 cmd: SessionUpdateControllerMulticastListCmd, 586 ) -> SessionUpdateControllerMulticastListRsp { 587 let session_handle = cmd.get_session_token(); 588 589 log::debug!( 590 "[{}:0x{:x}] Session Update Controller Multicast List", 591 self.handle, 592 session_handle 593 ); 594 595 let Some(session) = self.sessions.get_mut(&session_handle) else { 596 return SessionUpdateControllerMulticastListRspBuilder { 597 status: uci::Status::ErrorSessionNotExist, 598 } 599 .build(); 600 }; 601 602 if (session.state != SessionState::SessionStateActive 603 && session.state != SessionState::SessionStateIdle) 604 || session.app_config.device_type != Some(DeviceType::Controller) 605 || session.app_config.multi_node_mode != Some(MultiNodeMode::OneToMany) 606 { 607 return SessionUpdateControllerMulticastListRspBuilder { 608 status: uci::Status::Rejected, 609 } 610 .build(); 611 } 612 let action = cmd.get_action(); 613 let mut dst_addresses = session.app_config.dst_mac_address.clone(); 614 let new_controlees: Vec<Controlee> = match action { 615 UpdateMulticastListAction::AddControlee 616 | UpdateMulticastListAction::RemoveControlee => { 617 if let Ok(packet) = 618 SessionUpdateControllerMulticastListCmdPayload::parse(cmd.get_payload()) 619 { 620 packet 621 .controlees 622 .iter() 623 .map(|controlee| controlee.into()) 624 .collect() 625 } else { 626 return SessionUpdateControllerMulticastListRspBuilder { 627 status: uci::Status::SyntaxError, 628 } 629 .build(); 630 } 631 } 632 UpdateMulticastListAction::AddControleeWithShortSubSessionKey => { 633 if let Ok(packet) = 634 SessionUpdateControllerMulticastListCmd_2_0_16_Byte_Payload::parse( 635 cmd.get_payload(), 636 ) 637 { 638 packet 639 .controlees 640 .iter() 641 .map(|controlee| controlee.into()) 642 .collect() 643 } else { 644 return SessionUpdateControllerMulticastListRspBuilder { 645 status: uci::Status::SyntaxError, 646 } 647 .build(); 648 } 649 } 650 UpdateMulticastListAction::AddControleeWithExtendedSubSessionKey => { 651 if let Ok(packet) = 652 SessionUpdateControllerMulticastListCmd_2_0_32_Byte_Payload::parse( 653 cmd.get_payload(), 654 ) 655 { 656 packet 657 .controlees 658 .iter() 659 .map(|controlee| controlee.into()) 660 .collect() 661 } else { 662 return SessionUpdateControllerMulticastListRspBuilder { 663 status: uci::Status::SyntaxError, 664 } 665 .build(); 666 } 667 } 668 }; 669 let mut controlee_status = Vec::new(); 670 let mut status = uci::Status::Ok; 671 672 match action { 673 UpdateMulticastListAction::AddControlee 674 | UpdateMulticastListAction::AddControleeWithShortSubSessionKey 675 | UpdateMulticastListAction::AddControleeWithExtendedSubSessionKey => { 676 new_controlees.iter().for_each(|controlee| { 677 let mut update_status = MulticastUpdateStatus::OkMulticastListUpdate; 678 if !dst_addresses.contains(&controlee.short_address) { 679 if dst_addresses.len() == MAX_NUMBER_OF_CONTROLEES { 680 status = uci::Status::ErrorMulticastListFull; 681 update_status = MulticastUpdateStatus::ErrorMulticastListFull; 682 } else if (action 683 == UpdateMulticastListAction::AddControleeWithShortSubSessionKey 684 || action 685 == UpdateMulticastListAction::AddControleeWithExtendedSubSessionKey) 686 && session.app_config.sts_config 687 != uci::StsConfig::ProvisionedForResponderSubSessionKey 688 { 689 // If Action is 0x02 or 0x03 for STS_CONFIG values other than 690 // 0x04, the UWBS shall return SESSION_UPDATE_CONTROLLER_MULTICAST_LIST_NTF 691 // with Status set to STATUS_ERROR_SUB_SESSION_KEY_NOT_APPLICABLE for each 692 // Controlee in the Controlee List. 693 status = uci::Status::Failed; 694 update_status = MulticastUpdateStatus::ErrorSubSessionKeyNotApplicable; 695 } else { 696 dst_addresses.push(controlee.short_address); 697 }; 698 } 699 controlee_status.push(ControleeStatus { 700 mac_address: match controlee.short_address { 701 MacAddress::Short(address) => address, 702 MacAddress::Extended(_) => panic!("Extended address is not supported!"), 703 }, 704 status: update_status, 705 }); 706 }); 707 } 708 UpdateMulticastListAction::RemoveControlee => { 709 new_controlees.iter().for_each(|controlee: &Controlee| { 710 let pica_tx = self.pica_tx.clone(); 711 let address = controlee.short_address; 712 let attempt_count = session.app_config.in_band_termination_attempt_count; 713 let mut update_status = MulticastUpdateStatus::OkMulticastListUpdate; 714 if !dst_addresses.contains(&address) { 715 status = uci::Status::Failed; 716 update_status = MulticastUpdateStatus::ErrorKeyFetchFail; 717 } else { 718 dst_addresses.retain(|value| *value != address); 719 // If IN_BAND_TERMINATION_ATTEMPT_COUNT is not equal to 0x00, then the 720 // UWBS shall transmit the RCM with the “Stop Ranging” bit set to ‘1’ 721 // for IN_BAND_TERMINATION_ATTEMPT_COUNT times to the corresponding 722 // Controlee. 723 if attempt_count != 0 { 724 tokio::spawn(async move { 725 for _ in 0..attempt_count { 726 pica_tx 727 .send(PicaCommand::StopRanging(address, session_handle)) 728 .await 729 .unwrap() 730 } 731 }); 732 } 733 } 734 controlee_status.push(ControleeStatus { 735 mac_address: match address { 736 MacAddress::Short(addr) => addr, 737 MacAddress::Extended(_) => panic!("Extended address is not supported!"), 738 }, 739 status: update_status, 740 }); 741 }); 742 // Following requirements are applicable when the action is set to Delete (Action field set to 0x01): 743 // When the command is received while the Session State is SESSION_STATE_ACTIVE, 744 // For requested Controlees present in the multicast list, 745 // UWBS shall send the SESSION_UPDATE_CONTROLLER_MULTICAST_LIST_NTF and the 746 // corresponding Controlee status shall be set to STATUS_OK_MULTICAST_LIST_UPDATE 747 // in the Status List of SESSION_UPDATE_CONTROLLER_MULTICAST_LIST_NTF. 748 if session.state == SessionState::SessionStateActive { 749 let tx = self.tx.clone(); 750 tokio::spawn(async move { 751 // Sleep for 5ms to make sure the notification is not being 752 // sent before the response. 753 // TODO(#84) remove the sleep. 754 time::sleep(Duration::from_millis(5)).await; 755 tx.send( 756 SessionUpdateControllerMulticastListNtfBuilder { 757 controlee_status, 758 session_token: session_handle, 759 } 760 .build() 761 .encode_to_vec() 762 .unwrap(), 763 ) 764 .unwrap() 765 }); 766 } 767 } 768 } 769 session.app_config.number_of_controlees = dst_addresses.len() as u8; 770 session.app_config.dst_mac_address = dst_addresses.clone(); 771 // If the multicast list becomes empty, the UWBS shall move the session to 772 // SESSION_STATE_IDLE by sending the SESSION_STATUS_NTF with Reason Code 773 // set to ERROR_INVALID_NUM_OF_CONTROLEES. 774 if session.app_config.dst_mac_address.is_empty() { 775 session.set_state( 776 SessionState::SessionStateIdle, 777 ReasonCode::ErrorInvalidNumOfControlees, 778 ) 779 } 780 SessionUpdateControllerMulticastListRspBuilder { status }.build() 781 } 782 session_start(&mut self, cmd: SessionStartCmd) -> SessionStartRsp783 fn session_start(&mut self, cmd: SessionStartCmd) -> SessionStartRsp { 784 let session_id = cmd.get_session_id(); 785 786 log::debug!("[{}:0x{:x}] Session Start", self.handle, session_id); 787 788 let Some(session) = self.sessions.get_mut(&session_id) else { 789 return SessionStartRspBuilder { 790 status: uci::Status::ErrorSessionNotExist, 791 } 792 .build(); 793 }; 794 795 if session.state != SessionState::SessionStateIdle { 796 return SessionStartRspBuilder { 797 status: uci::Status::ErrorSessionNotConfigured, 798 } 799 .build(); 800 } 801 802 assert!(session.ranging_task.is_none()); 803 804 let ranging_interval = 805 time::Duration::from_millis(session.app_config.ranging_duration as u64); 806 807 let tx = self.pica_tx.clone(); 808 let handle = self.handle; 809 session.ranging_task = Some(tokio::spawn(async move { 810 loop { 811 time::sleep(ranging_interval).await; 812 tx.send(PicaCommand::Ranging(handle, session_id)) 813 .await 814 .unwrap(); 815 } 816 })); 817 818 session.set_state( 819 SessionState::SessionStateActive, 820 ReasonCode::StateChangeWithSessionManagementCommands, 821 ); 822 823 self.n_active_sessions += 1; 824 self.set_state(DeviceState::DeviceStateActive); 825 826 SessionStartRspBuilder { 827 status: uci::Status::Ok, 828 } 829 .build() 830 } 831 session_stop(&mut self, cmd: SessionStopCmd) -> SessionStopRsp832 fn session_stop(&mut self, cmd: SessionStopCmd) -> SessionStopRsp { 833 let session_id = cmd.get_session_id(); 834 835 log::debug!("[{}:0x{:x}] Session Stop", self.handle, session_id); 836 837 let Some(session) = self.sessions.get_mut(&session_id) else { 838 return SessionStopRspBuilder { 839 status: uci::Status::ErrorSessionNotExist, 840 } 841 .build(); 842 }; 843 844 if session.state != SessionState::SessionStateActive { 845 return SessionStopRspBuilder { 846 status: uci::Status::ErrorSessionActive, 847 } 848 .build(); 849 } 850 851 session.stop_ranging_task(); 852 session.set_state( 853 SessionState::SessionStateIdle, 854 ReasonCode::StateChangeWithSessionManagementCommands, 855 ); 856 857 self.n_active_sessions -= 1; 858 if self.n_active_sessions == 0 { 859 self.set_state(DeviceState::DeviceStateReady); 860 } 861 862 SessionStopRspBuilder { 863 status: uci::Status::Ok, 864 } 865 .build() 866 } 867 session_get_ranging_count( &self, cmd: SessionGetRangingCountCmd, ) -> SessionGetRangingCountRsp868 fn session_get_ranging_count( 869 &self, 870 cmd: SessionGetRangingCountCmd, 871 ) -> SessionGetRangingCountRsp { 872 let session_id = cmd.get_session_id(); 873 874 log::debug!( 875 "[{}:0x{:x}] Session Get Ranging Count", 876 self.handle, 877 session_id 878 ); 879 880 let Some(session) = self.sessions.get(&session_id) else { 881 return SessionGetRangingCountRspBuilder { 882 status: uci::Status::ErrorSessionNotExist, 883 count: 0, 884 } 885 .build(); 886 }; 887 888 SessionGetRangingCountRspBuilder { 889 status: uci::Status::Ok, 890 count: session.sequence_number, 891 } 892 .build() 893 } 894 android_set_country_code( &mut self, cmd: AndroidSetCountryCodeCmd, ) -> AndroidSetCountryCodeRsp895 fn android_set_country_code( 896 &mut self, 897 cmd: AndroidSetCountryCodeCmd, 898 ) -> AndroidSetCountryCodeRsp { 899 let country_code = *cmd.get_country_code(); 900 log::debug!("[{}] Set country code", self.handle); 901 log::debug!(" country_code={},{}", country_code[0], country_code[1]); 902 903 self.country_code = country_code; 904 AndroidSetCountryCodeRspBuilder { 905 status: uci::Status::Ok, 906 } 907 .build() 908 } 909 android_get_power_stats( &mut self, _cmd: AndroidGetPowerStatsCmd, ) -> AndroidGetPowerStatsRsp910 fn android_get_power_stats( 911 &mut self, 912 _cmd: AndroidGetPowerStatsCmd, 913 ) -> AndroidGetPowerStatsRsp { 914 log::debug!("[{}] Get power stats", self.handle); 915 916 // TODO 917 AndroidGetPowerStatsRspBuilder { 918 stats: PowerStats { 919 status: uci::Status::Ok, 920 idle_time_ms: 0, 921 tx_time_ms: 0, 922 rx_time_ms: 0, 923 total_wake_count: 0, 924 }, 925 } 926 .build() 927 } 928 data_message_snd(&mut self, data: DataPacket) -> ControlPacket929 pub fn data_message_snd(&mut self, data: DataPacket) -> ControlPacket { 930 log::debug!("[{}] data_message_send", self.handle); 931 match data.specialize() { 932 DataPacketChild::DataMessageSnd(data_msg_snd) => { 933 let session_token = data_msg_snd.get_session_handle(); 934 if let Some(session) = self.session_mut(session_token) { 935 session.data_message_snd(data_msg_snd) 936 } else { 937 SessionDataTransferStatusNtfBuilder { 938 session_token, 939 status: DataTransferNtfStatusCode::UciDataTransferStatusErrorRejected, 940 tx_count: 1, // TODO: support for retries? 941 uci_sequence_number: 0, 942 } 943 .build() 944 .into() 945 } 946 } 947 DataPacketChild::DataMessageRcv(data_msg_rcv) => { 948 // This function should not be passed anything besides DataMessageSnd 949 let session_token = data_msg_rcv.get_session_handle(); 950 SessionDataTransferStatusNtfBuilder { 951 session_token, 952 status: DataTransferNtfStatusCode::UciDataTransferStatusInvalidFormat, 953 tx_count: 1, // TODO: support for retries? 954 uci_sequence_number: 0, 955 } 956 .build() 957 .into() 958 } 959 _ => { 960 unimplemented!() 961 } 962 } 963 } 964 receive_command(&mut self, cmd: ControlPacket) -> ControlPacket965 fn receive_command(&mut self, cmd: ControlPacket) -> ControlPacket { 966 use AndroidPacketChild::*; 967 use ControlPacketChild::*; 968 use CorePacketChild::*; 969 use SessionConfigPacketChild::*; 970 use SessionControlPacketChild::*; 971 972 match cmd.specialize() { 973 CorePacket(cmd) => match cmd.specialize() { 974 CoreDeviceResetCmd(cmd) => self.core_device_reset(cmd).into(), 975 CoreGetDeviceInfoCmd(cmd) => self.core_get_device_info(cmd).into(), 976 CoreGetCapsInfoCmd(cmd) => self.core_get_caps_info(cmd).into(), 977 CoreSetConfigCmd(cmd) => self.core_set_config(cmd).into(), 978 CoreGetConfigCmd(cmd) => self.core_get_config(cmd).into(), 979 _ => unimplemented!("Unsupported Core oid {:?}", cmd.get_oid()), 980 }, 981 SessionConfigPacket(cmd) => match cmd.specialize() { 982 SessionInitCmd(cmd) => self.session_init(cmd).into(), 983 SessionDeinitCmd(cmd) => self.session_deinit(cmd).into(), 984 SessionGetCountCmd(cmd) => self.session_get_count(cmd).into(), 985 SessionSetAppConfigCmd(cmd) => self.session_set_app_config(cmd).into(), 986 SessionGetAppConfigCmd(cmd) => self.session_get_app_config(cmd).into(), 987 SessionGetStateCmd(cmd) => self.session_get_state(cmd).into(), 988 SessionUpdateControllerMulticastListCmd(cmd) => { 989 self.session_update_controller_multicast_list(cmd).into() 990 } 991 _ => unimplemented!("Unsupported Session Config oid {:?}", cmd.get_oid()), 992 }, 993 SessionControlPacket(cmd) => match cmd.specialize() { 994 SessionStartCmd(cmd) => self.session_start(cmd).into(), 995 SessionStopCmd(cmd) => self.session_stop(cmd).into(), 996 SessionGetRangingCountCmd(cmd) => self.session_get_ranging_count(cmd).into(), 997 _ => unimplemented!("Unsupported Session Control oid {:?}", cmd.get_oid()), 998 }, 999 AndroidPacket(cmd) => match cmd.specialize() { 1000 AndroidSetCountryCodeCmd(cmd) => self.android_set_country_code(cmd).into(), 1001 AndroidGetPowerStatsCmd(cmd) => self.android_get_power_stats(cmd).into(), 1002 _ => unimplemented!("Unsupported Android oid {:?}", cmd.get_oid()), 1003 }, 1004 ControlPacketChild::Payload(_) 1005 if matches!( 1006 cmd.get_mt(), 1007 uci::MessageType::Response | uci::MessageType::Notification 1008 ) => 1009 { 1010 unreachable!("Unhandled control messsage with type {:?}", cmd.get_mt()); 1011 } 1012 ControlPacketChild::Payload(payload) => { 1013 // [UCI] 4.3.2 Exception Handling for Control Messages 1014 // The UWBS shall respond to an unknown Command (unknown GID 1015 // or OID) with a Response having the same GID and OID field 1016 // values as the Command, followed by a Status field with the 1017 // value of STATUS_UNKNOWN_GID/STATUS_UNKNOWN_OID respectively 1018 // and no additional fields. 1019 log::error!("Unsupported gid {:?}", cmd.get_gid()); 1020 ControlPacketBuilder { 1021 mt: uci::MessageType::Response, 1022 gid: cmd.get_gid(), 1023 payload: Some( 1024 vec![payload[0], payload[1], 0x1, uci::Status::UnknownGid.into()].into(), 1025 ), 1026 } 1027 .build() 1028 } 1029 ControlPacketChild::None => { 1030 unreachable!() 1031 } 1032 } 1033 } 1034 receive_packet(&mut self, packet: Vec<u8>)1035 pub fn receive_packet(&mut self, packet: Vec<u8>) { 1036 let mt = parse_message_type(packet[0]); 1037 match mt { 1038 MessageType::Data => match DataPacket::parse(&packet) { 1039 Ok(packet) => { 1040 let notification = self.data_message_snd(packet); 1041 self.send_control(notification) 1042 } 1043 Err(err) => log::error!("failed to parse incoming Data packet: {}", err), 1044 }, 1045 MessageType::Command => { 1046 match ControlPacket::parse(&packet) { 1047 // Parsing error. Determine what error response should be 1048 // returned to the host: 1049 // - response and notifications are ignored, no response 1050 // - if the group id is not known, STATUS_UNKNOWN_GID, 1051 // - otherwise, and to simplify the code, STATUS_UNKNOWN_OID is 1052 // always returned. That means that malformed commands 1053 // get the same status code, instead of 1054 // STATUS_SYNTAX_ERROR. 1055 Err(_) => { 1056 let group_id = packet[0] & 0xf; 1057 let opcode_id = packet[1] & 0x3f; 1058 1059 let status = if GroupId::try_from(group_id).is_ok() { 1060 uci::Status::UnknownOid 1061 } else { 1062 uci::Status::UnknownGid 1063 }; 1064 // The PDL generated code cannot be used to generate 1065 // responses with invalid group identifiers. 1066 let response = vec![ 1067 (u8::from(MessageType::Response) << 5) | group_id, 1068 opcode_id, 1069 0, 1070 1, 1071 status.into(), 1072 ]; 1073 self.send_raw_control(response) 1074 } 1075 1076 // Parsing success, ignore non command packets. 1077 Ok(cmd) => { 1078 let response = self.receive_command(cmd); 1079 self.send_control(response) 1080 } 1081 } 1082 } 1083 1084 // Message types for notifications and responses ignored 1085 // by the controller. 1086 _ => log::warn!("received unexpected packet of MT {:?}", mt), 1087 } 1088 } 1089 } 1090 1091 struct Controlee { 1092 short_address: MacAddress, 1093 #[allow(dead_code)] 1094 sub_session_id: u32, 1095 #[allow(dead_code)] 1096 session_key: SubSessionKey, 1097 } 1098 1099 impl From<&uci::Controlee> for Controlee { from(value: &uci::Controlee) -> Self1100 fn from(value: &uci::Controlee) -> Self { 1101 Controlee { 1102 short_address: MacAddress::Short(value.short_address), 1103 sub_session_id: value.subsession_id, 1104 session_key: SubSessionKey::None, 1105 } 1106 } 1107 } 1108 1109 impl From<&uci::Controlee_V2_0_16_Byte_Version> for Controlee { from(value: &uci::Controlee_V2_0_16_Byte_Version) -> Self1110 fn from(value: &uci::Controlee_V2_0_16_Byte_Version) -> Self { 1111 Controlee { 1112 short_address: MacAddress::Short(value.short_address), 1113 sub_session_id: value.subsession_id, 1114 session_key: SubSessionKey::Short(value.subsession_key), 1115 } 1116 } 1117 } 1118 1119 impl From<&uci::Controlee_V2_0_32_Byte_Version> for Controlee { from(value: &uci::Controlee_V2_0_32_Byte_Version) -> Self1120 fn from(value: &uci::Controlee_V2_0_32_Byte_Version) -> Self { 1121 Controlee { 1122 short_address: MacAddress::Short(value.short_address), 1123 sub_session_id: value.subsession_id, 1124 session_key: SubSessionKey::Extended(value.subsession_key), 1125 } 1126 } 1127 } 1128