Skip to content

Commit

Permalink
Sync uci packets with UWB AOSP backend. (#30)
Browse files Browse the repository at this point in the history 
* Sync uci packets with UWB AOSP backend.
  • Loading branch information
@bobwzy
bobwzy authored Aug 4, 2023
1 parent 0e967f4 commit 84627c6
Show file tree
Hide file tree
Showing 5 changed files with 21,837 additions and 11,847 deletions.
169 changes: 85 additions & 84 deletions src/device.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@ use std::iter::Extend;

use tokio::sync::mpsc;

use num_traits::FromPrimitive;

use super::session::{Session, MAX_SESSION};

pub const MAX_DEVICE: usize = 4;
Expand Down Expand Up @@ -78,9 +76,9 @@ pub struct Device {
/// [UCI] 5. UWBS Device State Machine
state: DeviceState,
sessions: HashMap<u32, Session>,
pub tx: mpsc::Sender<UciPacketPacket>,
pub tx: mpsc::Sender<UciPacket>,
pica_tx: mpsc::Sender<PicaCommand>,
config: HashMap<u8, Vec<u8>>,
config: HashMap<DeviceConfigId, Vec<u8>>,
country_code: [u8; 2],

n_active_sessions: usize,
Expand All @@ -89,7 +87,7 @@ pub struct Device {
impl Device {
pub fn new(
device_handle: usize,
tx: mpsc::Sender<UciPacketPacket>,
tx: mpsc::Sender<UciPacket>,
pica_tx: mpsc::Sender<PicaCommand>,
) -> Self {
let mac_address = {
Expand Down Expand Up @@ -140,10 +138,10 @@ impl Device {

// The fira norm specify to send a response, then reset, then
// send a notification once the reset is done
fn command_device_reset(&mut self, cmd: DeviceResetCmdPacket) -> DeviceResetRspPacket {
fn command_device_reset(&mut self, cmd: DeviceResetCmd) -> DeviceResetRsp {
let reset_config = cmd.get_reset_config();
println!("[{}] DeviceReset", self.handle);
println!(" reset_config={}", reset_config);
println!(" reset_config={:?}", reset_config);

let status = match reset_config {
ResetConfig::UwbsReset => StatusCode::UciStatusOk,
Expand All @@ -154,7 +152,7 @@ impl Device {
DeviceResetRspBuilder { status }.build()
}

fn command_get_device_info(&self, _cmd: GetDeviceInfoCmdPacket) -> GetDeviceInfoRspPacket {
fn command_get_device_info(&self, _cmd: GetDeviceInfoCmd) -> GetDeviceInfoRsp {
// TODO: Implement a fancy build time state machine instead of crash at runtime
println!("[{}] GetDeviceInfo", self.handle);
assert_eq!(self.state, DeviceState::DeviceStateReady);
Expand All @@ -169,7 +167,7 @@ impl Device {
.build()
}

pub fn command_get_caps_info(&self, cmd: GetCapsInfoCmdPacket) -> GetCapsInfoRspPacket {
pub fn command_get_caps_info(&self, cmd: GetCapsInfoCmd) -> GetCapsInfoRsp {
println!("[{}] GetCapsInfo", self.handle);
assert_eq!(
cmd.get_packet_boundary_flag(),
Expand All @@ -192,7 +190,7 @@ impl Device {
.build()
}

pub fn command_set_config(&mut self, cmd: SetConfigCmdPacket) -> SetConfigRspPacket {
pub fn command_set_config(&mut self, cmd: SetConfigCmd) -> SetConfigRsp {
println!("[{}] SetConfig", self.handle);
assert_eq!(self.state, DeviceState::DeviceStateReady); // UCI 6.3
assert_eq!(
Expand All @@ -201,23 +199,12 @@ impl Device {
"Boundary flag is true, implement fragmentation"
);

let (valid_parameters, invalid_config_status) = cmd.get_parameters().iter().fold(
let (valid_parameters, invalid_config_status) = cmd.get_tlvs().iter().fold(
(HashMap::new(), Vec::new()),
|(mut valid_parameters, mut invalid_config_status), param| {
let id = param.id;
match DeviceConfigId::from_u8(id) {
Some(_) => {
// TODO: DeviceState is a read only parameter
valid_parameters.insert(param.id, param.value.clone());
}
None => {
// TODO: silently ignore vendor parameter
invalid_config_status.push(DeviceConfigStatus {
parameter_id: id,
status: StatusCode::UciStatusInvalidParam,
})
}
};
|(mut valid_parameters, invalid_config_status), param| {
// TODO: DeviceState is a read only parameter
valid_parameters.insert(param.cfg_id, param.v.clone());

(valid_parameters, invalid_config_status)
},
);
Expand All @@ -229,17 +216,21 @@ impl Device {
(StatusCode::UciStatusInvalidParam, invalid_config_status)
};

SetConfigRspBuilder { status, parameters }.build()
SetConfigRspBuilder {
cfg_status: parameters,
status,
}
.build()
}

pub fn command_get_config(&self, cmd: GetConfigCmdPacket) -> GetConfigRspPacket {
pub fn command_get_config(&self, cmd: GetConfigCmd) -> GetConfigRsp {
println!("[{}] GetConfig", self.handle);
assert_eq!(
cmd.get_packet_boundary_flag(),
PacketBoundaryFlag::Complete,
"Boundary flag is true, implement fragmentation"
);
let ids = cmd.get_parameter_ids();
let ids = cmd.get_cfg_id();

// TODO: do this config shall be set on device reset
let (valid_parameters, invalid_parameters) = ids.iter().fold(
Expand All @@ -250,15 +241,18 @@ impl Device {
// If the status code is ok, return the params
// If there is at least one invalid param, return the list of invalid params
// If the ID is not present in our config, return the Type with length = 0
match self.config.get(id) {
Some(value) => valid_parameters.push(DeviceParameter {
id: *id,
value: value.clone(),
}),
None => invalid_parameters.push(DeviceParameter {
id: *id,
value: Vec::new(),
}),
match DeviceConfigId::try_from(*id) {
Ok(cfg_id) => match self.config.get(&cfg_id) {
Some(value) => valid_parameters.push(DeviceConfigTlv {
cfg_id,
v: value.clone(),
}),
None => invalid_parameters.push(DeviceConfigTlv {
cfg_id,
v: Vec::new(),
}),
},
Err(_) => println!("Failed to parse config id: {:?}", id),
}

(valid_parameters, invalid_parameters)
Expand All @@ -271,16 +265,20 @@ impl Device {
(StatusCode::UciStatusInvalidParam, invalid_parameters)
};

GetConfigRspBuilder { status, parameters }.build()
GetConfigRspBuilder {
status,
tlvs: parameters,
}
.build()
}

fn command_session_init(&mut self, cmd: SessionInitCmdPacket) -> SessionInitRspPacket {
fn command_session_init(&mut self, cmd: SessionInitCmd) -> SessionInitRsp {
let session_id = cmd.get_session_id();
let session_type = cmd.get_session_type();

println!("[{}] Session init", self.handle);
println!(" session_id=0x{:x}", session_id);
println!(" session_type={}", session_type);
println!(" session_type={:?}", session_type);

let status = if self.sessions.len() >= MAX_SESSION {
StatusCode::UciStatusMaxSessionsExceeded
Expand All @@ -307,8 +305,8 @@ impl Device {
SessionInitRspBuilder { status }.build()
}

fn command_session_deinit(&mut self, cmd: SessionDeinitCmdPacket) -> SessionDeinitRspPacket {
let session_id = cmd.get_session_id();
fn command_session_deinit(&mut self, cmd: SessionDeinitCmd) -> SessionDeinitRsp {
let session_id = cmd.get_session_token();
println!("[{}] Session deinit", self.handle);
println!(" session_id=0x{:x}", session_id);

Expand All @@ -321,10 +319,7 @@ impl Device {
SessionDeinitRspBuilder { status }.build()
}

fn command_session_get_count(
&self,
_cmd: SessionGetCountCmdPacket,
) -> SessionGetCountRspPacket {
fn command_session_get_count(&self, _cmd: SessionGetCountCmd) -> SessionGetCountRsp {
println!("[{}] Session get count", self.handle);

SessionGetCountRspBuilder {
Expand All @@ -336,8 +331,8 @@ impl Device {

fn command_set_country_code(
&mut self,
cmd: AndroidSetCountryCodeCmdPacket,
) -> AndroidSetCountryCodeRspPacket {
cmd: AndroidSetCountryCodeCmd,
) -> AndroidSetCountryCodeRsp {
let country_code = *cmd.get_country_code();
println!("[{}] Set country code", self.handle);
println!(" country_code={},{}", country_code[0], country_code[1]);
Expand All @@ -351,8 +346,8 @@ impl Device {

fn command_get_power_stats(
&mut self,
_cmd: AndroidGetPowerStatsCmdPacket,
) -> AndroidGetPowerStatsRspPacket {
_cmd: AndroidGetPowerStatsCmd,
) -> AndroidGetPowerStatsRsp {
println!("[{}] Get power stats", self.handle);

// TODO
Expand All @@ -368,7 +363,7 @@ impl Device {
.build()
}

pub fn command(&mut self, cmd: UciCommandPacket) -> UciResponsePacket {
pub fn command(&mut self, cmd: UciCommand) -> UciResponse {
match cmd.specialize() {
// Handle commands for this device
UciCommandChild::CoreCommand(core_command) => match core_command.specialize() {
Expand All @@ -380,28 +375,32 @@ impl Device {
_ => panic!("Unsupported core command"),
},
// Handle commands for session management
UciCommandChild::SessionCommand(session_command) => {
UciCommandChild::SessionConfigCommand(session_command) => {
// Session commands directly handled at Device level
match session_command.specialize() {
SessionCommandChild::SessionInitCmd(cmd) => {
SessionConfigCommandChild::SessionInitCmd(cmd) => {
return self.command_session_init(cmd).into();
}
SessionCommandChild::SessionDeinitCmd(cmd) => {
SessionConfigCommandChild::SessionDeinitCmd(cmd) => {
return self.command_session_deinit(cmd).into();
}
SessionCommandChild::SessionGetCountCmd(cmd) => {
SessionConfigCommandChild::SessionGetCountCmd(cmd) => {
return self.command_session_get_count(cmd).into();
}
_ => {}
}

// Common code for retrieving the session_id in the command
let session_id = match session_command.specialize() {
SessionCommandChild::SessionSetAppConfigCmd(cmd) => cmd.get_session_id(),
SessionCommandChild::SessionGetAppConfigCmd(cmd) => cmd.get_session_id(),
SessionCommandChild::SessionGetStateCmd(cmd) => cmd.get_session_id(),
SessionCommandChild::SessionUpdateControllerMulticastListCmd(cmd) => {
cmd.get_session_id()
SessionConfigCommandChild::SessionSetAppConfigCmd(cmd) => {
cmd.get_session_token()
}
SessionConfigCommandChild::SessionGetAppConfigCmd(cmd) => {
cmd.get_session_token()
}
SessionConfigCommandChild::SessionGetStateCmd(cmd) => cmd.get_session_token(),
SessionConfigCommandChild::SessionUpdateControllerMulticastListCmd(cmd) => {
cmd.get_session_token()
}
_ => panic!("Unsupported session command type"),
};
Expand All @@ -410,10 +409,10 @@ impl Device {
// There is a session matching the session_id in the command
// Pass the command through
match session_command.specialize() {
SessionCommandChild::SessionSetAppConfigCmd(_)
| SessionCommandChild::SessionGetAppConfigCmd(_)
| SessionCommandChild::SessionGetStateCmd(_)
| SessionCommandChild::SessionUpdateControllerMulticastListCmd(_) => {
SessionConfigCommandChild::SessionSetAppConfigCmd(_)
| SessionConfigCommandChild::SessionGetAppConfigCmd(_)
| SessionConfigCommandChild::SessionGetStateCmd(_)
| SessionConfigCommandChild::SessionUpdateControllerMulticastListCmd(_) => {
session.session_command(session_command).into()
}
_ => panic!("Unsupported session command"),
Expand All @@ -422,29 +421,31 @@ impl Device {
// There is no session matching the session_id in the command
let status = StatusCode::UciStatusSessionNotExist;
match session_command.specialize() {
SessionCommandChild::SessionSetAppConfigCmd(_) => {
SessionConfigCommandChild::SessionSetAppConfigCmd(_) => {
SessionSetAppConfigRspBuilder {
cfg_status: Vec::new(),
status,
parameters: Vec::new(),
}
.build()
.into()
}
SessionCommandChild::SessionGetAppConfigCmd(_) => {
SessionConfigCommandChild::SessionGetAppConfigCmd(_) => {
SessionGetAppConfigRspBuilder {
status,
parameters: Vec::new(),
tlvs: Vec::new(),
}
.build()
.into()
}
SessionCommandChild::SessionGetStateCmd(_) => SessionGetStateRspBuilder {
status,
session_state: SessionState::SessionStateDeinit,
SessionConfigCommandChild::SessionGetStateCmd(_) => {
SessionGetStateRspBuilder {
status,
session_state: SessionState::SessionStateDeinit,
}
.build()
.into()
}
.build()
.into(),
SessionCommandChild::SessionUpdateControllerMulticastListCmd(_) => {
SessionConfigCommandChild::SessionUpdateControllerMulticastListCmd(_) => {
SessionUpdateControllerMulticastListRspBuilder { status }
.build()
.into()
Expand All @@ -453,19 +454,19 @@ impl Device {
}
}
}
UciCommandChild::RangingCommand(ranging_command) => {
UciCommandChild::SessionControlCommand(ranging_command) => {
let session_id = ranging_command.get_session_id();
if let Some(session) = self.get_session_mut(session_id) {
// Forward to the proper session
let response = session.ranging_command(ranging_command);
match response.specialize() {
RangingResponseChild::RangeStartRsp(rsp)
SessionControlResponseChild::SessionStartRsp(rsp)
if rsp.get_status() == StatusCode::UciStatusOk =>
{
self.n_active_sessions += 1;
self.set_state(DeviceState::DeviceStateActive);
}
RangingResponseChild::RangeStopRsp(rsp)
SessionControlResponseChild::SessionStopRsp(rsp)
if rsp.get_status() == StatusCode::UciStatusOk =>
{
assert!(self.n_active_sessions > 0);
Expand All @@ -480,14 +481,14 @@ impl Device {
} else {
let status = StatusCode::UciStatusSessionNotExist;
match ranging_command.specialize() {
RangingCommandChild::RangeStartCmd(_) => {
RangeStartRspBuilder { status }.build().into()
SessionControlCommandChild::SessionStartCmd(_) => {
SessionStartRspBuilder { status }.build().into()
}
RangingCommandChild::RangeStopCmd(_) => {
RangeStopRspBuilder { status }.build().into()
SessionControlCommandChild::SessionStopCmd(_) => {
SessionStopRspBuilder { status }.build().into()
}
RangingCommandChild::RangeGetRangingCountCmd(_) => {
RangeGetRangingCountRspBuilder { status, count: 0 }
SessionControlCommandChild::SessionGetRangingCountCmd(_) => {
SessionGetRangingCountRspBuilder { status, count: 0 }
.build()
.into()
}
Expand Down
Loading

0 comments on commit 84627c6

Please sign in to comment.