Skip to content

Commit

Permalink
Move packet parsing from lib.rs to device.rs
Browse files Browse the repository at this point in the history
  • Loading branch information
@hchataing
hchataing committed Feb 23, 2024
1 parent e5f3a94 commit a28b52a
Show file tree
Hide file tree
Showing 2 changed files with 71 additions and 106 deletions.
62 changes: 61 additions & 1 deletion src/device.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,6 +168,11 @@ impl Device {
}
}

// Send a response or notification to the Host.
fn send_control(&mut self, packet: impl Into<Vec<u8>>) {
let _ = self.tx.send(packet.into());
}

// 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: DeviceResetCmd) -> DeviceResetRsp {
Expand Down Expand Up @@ -428,7 +433,7 @@ impl Device {
}
}

pub fn command(&mut self, cmd: UciCommand) -> UciResponse {
fn receive_command(&mut self, cmd: UciCommand) -> UciResponse {
match cmd.specialize() {
// Handle commands for this device
UciCommandChild::CoreCommand(core_command) => match core_command.specialize() {
Expand Down Expand Up @@ -611,4 +616,59 @@ impl Device {
.build(),
}
}

pub fn receive_packet(&mut self, packet: Vec<u8>) {
let mt = parse_message_type(packet[0]);
match mt {
MessageType::Data => match DataPacket::parse(&packet) {
Ok(packet) => {
let notification = self.data_message_snd(packet);
self.send_control(notification)
}
Err(err) => log::error!("failed to parse incoming Data packet: {}", err),
},
MessageType::Command => {
match ControlPacket::parse(&packet) {
// Parsing error. Determine what error response should be
// returned to the host:
// - response and notifications are ignored, no response
// - if the group id is not known, STATUS_UNKNOWN_GID,
// - otherwise, and to simplify the code, STATUS_UNKNOWN_OID is
// always returned. That means that malformed commands
// get the same status code, instead of
// STATUS_SYNTAX_ERROR.
Err(_) => {
let group_id = packet[0] & 0xf;
let opcode_id = packet[1] & 0x3f;

let status = if GroupId::try_from(group_id).is_ok() {
StatusCode::UciStatusUnknownOid
} else {
StatusCode::UciStatusUnknownGid
};
// The PDL generated code cannot be used to generate
// responses with invalid group identifiers.
let response = vec![
(u8::from(MessageType::Response) << 5) | group_id,
opcode_id,
0,
1,
status.into(),
];
self.send_control(response)
}

// Parsing success, ignore non command packets.
Ok(cmd) => {
let response = self.receive_command(cmd.try_into().unwrap());
self.send_control(response)
}
}
}

// Message types for notifications and responses ignored
// by the controller.
_ => log::warn!("received unexpected packet of MT {:?}", mt),
}
}
}
115 changes: 10 additions & 105 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,6 @@
// limitations under the License.

use anyhow::Result;
use bytes::Bytes;
use pdl_runtime::Packet;
use serde::{Deserialize, Serialize};
use std::collections::HashMap;
use std::fmt::Display;
Expand Down Expand Up @@ -98,10 +96,8 @@ pub enum PicaCommand {
Ranging(usize, u32),
// Send an in-band request to stop ranging to a peer controlee identified by address and session id.
StopRanging(MacAddress, u32),
// Execute data message send for selected device and data.
UciData(usize, DataPacket),
// Execute UCI command received for selected device.
UciCommand(usize, UciCommand),
// UCI packet received for the selected device.
UciPacket(usize, Vec<u8>),
// Create Anchor
CreateAnchor(
MacAddress,
Expand All @@ -121,8 +117,7 @@ impl Display for PicaCommand {
PicaCommand::Disconnect(_) => "Disconnect",
PicaCommand::Ranging(_, _) => "Ranging",
PicaCommand::StopRanging(_, _) => "StopRanging",
PicaCommand::UciData(_, _) => "UciData",
PicaCommand::UciCommand(_, _) => "UciCommand",
PicaCommand::UciPacket(_, _) => "UciPacket",
PicaCommand::CreateAnchor(_, _) => "CreateAnchor",
PicaCommand::DestroyAnchor(_, _) => "DestroyAnchor",
};
Expand Down Expand Up @@ -158,67 +153,6 @@ struct Anchor {
mac_address: MacAddress,
}

/// Result of UCI packet parsing.
enum UciParseResult {
UciCommand(UciCommand),
UciData(DataPacket),
Err(Bytes),
Skip,
}

/// Parse incoming UCI packets.
/// Handle parsing errors by crafting a suitable error response packet.
fn parse_uci_packet(bytes: &[u8]) -> UciParseResult {
let message_type = parse_message_type(bytes[0]);
match message_type {
MessageType::Data => match DataPacket::parse(bytes) {
Ok(packet) => UciParseResult::UciData(packet),
Err(_) => UciParseResult::Skip,
},
_ => {
match ControlPacket::parse(bytes) {
// Parsing error. Determine what error response should be
// returned to the host:
// - response and notifications are ignored, no response
// - if the group id is not known, STATUS_UNKNOWN_GID,
// - otherwise, and to simplify the code, STATUS_UNKNOWN_OID is
// always returned. That means that malformed commands
// get the same status code, instead of
// STATUS_SYNTAX_ERROR.
Err(_) => {
let group_id = bytes[0] & 0xf;
let opcode_id = bytes[1] & 0x3f;

let status = match (message_type, GroupId::try_from(group_id)) {
(MessageType::Command, Ok(_)) => UciStatusCode::UciStatusUnknownOid,
(MessageType::Command, Err(_)) => UciStatusCode::UciStatusUnknownGid,
_ => return UciParseResult::Skip,
};
// The PDL generated code cannot be used to generate
// responses with invalid group identifiers.
let response = vec![
(u8::from(MessageType::Response) << 5) | group_id,
opcode_id,
0,
1,
status.into(),
];
UciParseResult::Err(response.into())
}

// Parsing success, ignore non command packets.
Ok(packet) => {
if let Ok(cmd) = packet.try_into() {
UciParseResult::UciCommand(cmd)
} else {
UciParseResult::Skip
}
}
}
}
}
}

fn make_measurement(
mac_address: &MacAddress,
local: RangingMeasurement,
Expand Down Expand Up @@ -312,7 +246,6 @@ impl Pica {

fn connect(&mut self, stream: TcpStream) {
let (packet_tx, mut packet_rx) = mpsc::unbounded_channel();
let invalid_packet_tx = packet_tx.clone();
let device_handle = self.counter;
let pica_tx = self.command_tx.clone();
let pcapng_dir = self.pcapng_dir.clone();
Expand Down Expand Up @@ -358,20 +291,10 @@ impl Pica {
// Read UCI packets sent from connected UWB host.
// Run associated command.
match uci_reader.read(&pcapng_file).await {
Ok(packet) => match parse_uci_packet(&packet) {
UciParseResult::UciCommand(cmd) => pica_tx
.send(PicaCommand::UciCommand(device_handle, cmd))
.await
.unwrap(),
UciParseResult::UciData(data) => pica_tx
.send(PicaCommand::UciData(device_handle, data))
.await
.unwrap(),
UciParseResult::Err(response) => {
invalid_packet_tx.send(response.into()).unwrap()
}
UciParseResult::Skip => (),
},
Ok(packet) => pica_tx
.send(PicaCommand::UciPacket(device_handle, packet))
.await
.unwrap(),
err => break err,
}
}
Expand Down Expand Up @@ -505,26 +428,9 @@ impl Pica {
session.clear_data();
}

fn uci_data(&mut self, device_handle: usize, data: DataPacket) {
fn uci_packet(&mut self, device_handle: usize, packet: Vec<u8>) {
match self.get_device_mut(device_handle) {
Some(device) => {
let response: SessionControlNotification = device.data_message_snd(data);
device.tx.send(response.into()).unwrap_or_else(|err| {
log::error!("Failed to send UCI data packet response: {}", err)
});
}
None => log::error!("Device {} not found", device_handle),
}
}

fn uci_command(&mut self, device_handle: usize, cmd: UciCommand) {
match self.get_device_mut(device_handle) {
Some(device) => {
let response: ControlPacket = device.command(cmd).into();
device.tx.send(response.to_vec()).unwrap_or_else(|err| {
log::error!("Failed to send UCI command response: {}", err)
});
}
Some(device) => device.receive_packet(packet),
None => log::error!("Device {} not found", device_handle),
}
}
Expand All @@ -538,8 +444,7 @@ impl Pica {
StopRanging(mac_address, session_id) => {
self.stop_controlee_ranging(&mac_address, session_id)
}
UciData(device_handle, data) => self.uci_data(device_handle, data),
UciCommand(device_handle, cmd) => self.uci_command(device_handle, cmd),
UciPacket(device_handle, packet) => self.uci_packet(device_handle, packet),
CreateAnchor(mac_address, pica_cmd_rsp_tx) => {
self.create_anchor(mac_address, pica_cmd_rsp_tx)
}
Expand Down

0 comments on commit a28b52a

Please sign in to comment.