use crate::protocol::{self, Command, EncodedFrame, FrameRef, Message, NodeId, Query, Response};
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct OperationId(u64);
impl OperationId {
pub const fn get(self) -> u64 {
self.0
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum OperationKind {
Command(Command),
Query(Query),
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum OperationState {
Prepared,
Dispatching,
Submitted,
Observed,
Failed,
TimedOut,
Cancelled,
Unknown,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct OperationReport {
pub id: OperationId,
pub kind: OperationKind,
pub deadline_ms: u64,
pub prepared_at_ms: u64,
pub dispatching_at_ms: Option<u64>,
pub submitted_at_ms: Option<u64>,
pub terminal_at_ms: Option<u64>,
pub state: OperationState,
pub response: Option<Response>,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum PrepareError {
Busy,
DeadlineElapsed {
now_ms: u64,
deadline_ms: u64,
},
ClockRollback {
previous_ms: u64,
now_ms: u64,
},
Encode(protocol::EncodeError),
IdExhausted,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum BeginSendError {
StaleOperation,
NotPrepared(OperationState),
ClockRollback {
previous_ms: u64,
now_ms: u64,
},
DeadlineElapsed,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum AttemptError {
ClockRollback,
DeadlineElapsed,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum ReportError {
StaleOperation,
Pending,
UnknownPending,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum IngestResult {
Unrelated,
Message(Message),
DecodeError(protocol::DecodeError),
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum ResponseKind {
Heartbeat,
MotorError,
EncoderError,
SensorlessError,
EncoderEstimates,
EncoderCount,
Iq,
SensorlessEstimates,
VbusVoltage,
}
impl ResponseKind {
const fn index(self) -> usize {
match self {
Self::Heartbeat => 0,
Self::MotorError => 1,
Self::EncoderError => 2,
Self::SensorlessError => 3,
Self::EncoderEstimates => 4,
Self::EncoderCount => 5,
Self::Iq => 6,
Self::SensorlessEstimates => 7,
Self::VbusVoltage => 8,
}
}
fn from_response(response: Response) -> Self {
match response {
Response::Heartbeat { .. } => Self::Heartbeat,
Response::MotorError(_) => Self::MotorError,
Response::EncoderError(_) => Self::EncoderError,
Response::SensorlessError(_) => Self::SensorlessError,
Response::EncoderEstimates { .. } => Self::EncoderEstimates,
Response::EncoderCount { .. } => Self::EncoderCount,
Response::Iq { .. } => Self::Iq,
Response::SensorlessEstimates { .. } => Self::SensorlessEstimates,
Response::VbusVoltage(_) => Self::VbusVoltage,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct CachedResponse {
pub response: Response,
pub received_at_ms: u64,
}
#[derive(Clone, Copy, Debug, Default)]
pub struct ResponseCache {
entries: [Option<CachedResponse>; 9],
}
impl ResponseCache {
pub fn get(&self, kind: ResponseKind) -> Option<CachedResponse> {
self.entries[kind.index()]
}
pub fn heartbeat_age_ms(&self, now_ms: u64) -> Option<u64> {
self.get(ResponseKind::Heartbeat)
.and_then(|entry| now_ms.checked_sub(entry.received_at_ms))
}
pub fn heartbeat_is_fresh(&self, now_ms: u64, max_age_ms: u64) -> bool {
matches!(self.heartbeat_age_ms(now_ms), Some(age) if age <= max_age_ms)
}
fn update(&mut self, response: Response, received_at_ms: u64) {
let index = ResponseKind::from_response(response).index();
if self.entries[index].is_none_or(|cached| cached.received_at_ms <= received_at_ms) {
self.entries[index] = Some(CachedResponse {
response,
received_at_ms,
});
}
}
}
struct ActiveOperation {
report: OperationReport,
frame: EncodedFrame,
}
pub struct Driver {
node: NodeId,
next_id: u64,
last_now_ms: Option<u64>,
active: Option<ActiveOperation>,
completed: Option<OperationReport>,
cache: ResponseCache,
}
impl Driver {
pub const fn new(node: NodeId) -> Self {
Self {
node,
next_id: 0,
last_now_ms: None,
active: None,
completed: None,
cache: ResponseCache { entries: [None; 9] },
}
}
pub const fn node(&self) -> NodeId {
self.node
}
pub const fn cache(&self) -> &ResponseCache {
&self.cache
}
pub fn prepare_command(
&mut self,
command: Command,
now_ms: u64,
deadline_ms: u64,
) -> Result<OperationId, PrepareError> {
self.prepare(OperationKind::Command(command), now_ms, deadline_ms)
}
pub fn prepare_query(
&mut self,
query: Query,
now_ms: u64,
deadline_ms: u64,
) -> Result<OperationId, PrepareError> {
self.prepare(OperationKind::Query(query), now_ms, deadline_ms)
}
pub fn begin_send(
&mut self,
id: OperationId,
now_ms: u64,
) -> Result<SendAttempt<'_>, BeginSendError> {
let Some(active) = self.active.as_ref() else {
return Err(BeginSendError::StaleOperation);
};
if active.report.id != id {
return Err(BeginSendError::StaleOperation);
}
if active.report.state != OperationState::Prepared {
return Err(BeginSendError::NotPrepared(active.report.state));
}
if let Some(previous_ms) = self.clock_rollback(now_ms) {
return Err(BeginSendError::ClockRollback {
previous_ms,
now_ms,
});
}
if now_ms >= active.report.deadline_ms {
self.commit_clock(now_ms);
self.finish_active(OperationState::TimedOut, now_ms, None);
return Err(BeginSendError::DeadlineElapsed);
}
self.commit_clock(now_ms);
let active = self
.active
.as_mut()
.expect("active operation checked above");
active.report.state = OperationState::Dispatching;
active.report.dispatching_at_ms = Some(now_ms);
Ok(SendAttempt {
driver: self,
id,
settled: false,
})
}
pub fn ingest(&mut self, frame: FrameRef<'_>, received_at_ms: u64) -> IngestResult {
let message = match protocol::decode(self.node, frame) {
Ok(Some(message)) => message,
Ok(None) => return IngestResult::Unrelated,
Err(error) => return IngestResult::DecodeError(error),
};
if let Message::Response(response) = message {
self.cache.update(response, received_at_ms);
self.observe_query(response, received_at_ms);
}
IngestResult::Message(message)
}
pub fn tick(&mut self, now_ms: u64) -> Result<(), PrepareError> {
if let Some(previous_ms) = self.clock_rollback(now_ms) {
return Err(PrepareError::ClockRollback {
previous_ms,
now_ms,
});
}
self.commit_clock(now_ms);
if self.active.as_ref().is_some_and(|active| {
now_ms >= active.report.deadline_ms
&& matches!(
active.report.state,
OperationState::Prepared | OperationState::Submitted
)
}) {
self.finish_active(OperationState::TimedOut, now_ms, None);
}
Ok(())
}
pub fn cancel(&mut self, id: OperationId, now_ms: u64) -> Result<(), BeginSendError> {
let Some(active) = self.active.as_ref() else {
return Err(BeginSendError::StaleOperation);
};
if active.report.id != id {
return Err(BeginSendError::StaleOperation);
}
if active.report.state != OperationState::Prepared {
return Err(BeginSendError::NotPrepared(active.report.state));
}
if let Some(previous_ms) = self.clock_rollback(now_ms) {
return Err(BeginSendError::ClockRollback {
previous_ms,
now_ms,
});
}
self.commit_clock(now_ms);
self.finish_active(OperationState::Cancelled, now_ms, None);
Ok(())
}
pub fn report(&self, id: OperationId) -> Result<OperationReport, ReportError> {
if let Some(active) = self.active.as_ref().filter(|active| active.report.id == id) {
return Ok(active.report);
}
if let Some(report) = self.completed.filter(|report| report.id == id) {
return Ok(report);
}
Err(ReportError::StaleOperation)
}
pub fn take_report(&mut self, id: OperationId) -> Result<OperationReport, ReportError> {
if let Some(active) = self.active.as_ref().filter(|active| active.report.id == id) {
return match active.report.state {
OperationState::Unknown => Err(ReportError::UnknownPending),
_ => Err(ReportError::Pending),
};
}
if self.completed.is_some_and(|report| report.id == id) {
return Ok(self
.completed
.take()
.expect("completed report checked above"));
}
Err(ReportError::StaleOperation)
}
pub fn acknowledge_unknown(&mut self, id: OperationId) -> Result<OperationReport, ReportError> {
let Some(active) = self.active.as_ref() else {
return Err(ReportError::StaleOperation);
};
if active.report.id != id {
return Err(ReportError::StaleOperation);
}
if active.report.state != OperationState::Unknown {
return Err(ReportError::Pending);
}
let report = self
.active
.take()
.expect("active operation checked above")
.report;
self.completed = Some(report);
Ok(report)
}
fn prepare(
&mut self,
kind: OperationKind,
now_ms: u64,
deadline_ms: u64,
) -> Result<OperationId, PrepareError> {
if self.active.is_some() || self.completed.is_some() {
return Err(PrepareError::Busy);
}
if let Some(previous_ms) = self.clock_rollback(now_ms) {
return Err(PrepareError::ClockRollback {
previous_ms,
now_ms,
});
}
if now_ms >= deadline_ms {
return Err(PrepareError::DeadlineElapsed {
now_ms,
deadline_ms,
});
}
let message = match kind {
OperationKind::Command(command) => Message::Command(command),
OperationKind::Query(query) => Message::Request(query),
};
let frame = protocol::encode(self.node, message).map_err(PrepareError::Encode)?;
let id = OperationId(self.next_id);
self.next_id = self
.next_id
.checked_add(1)
.ok_or(PrepareError::IdExhausted)?;
self.commit_clock(now_ms);
self.active = Some(ActiveOperation {
report: OperationReport {
id,
kind,
deadline_ms,
prepared_at_ms: now_ms,
dispatching_at_ms: None,
submitted_at_ms: None,
terminal_at_ms: None,
state: OperationState::Prepared,
response: None,
},
frame,
});
Ok(id)
}
fn observe_query(&mut self, response: Response, received_at_ms: u64) {
let Some(active) = self.active.as_ref() else {
return;
};
let is_match = matches!(active.report.kind, OperationKind::Query(query) if response_matches(query, response))
&& active.report.state == OperationState::Submitted
&& active
.report
.submitted_at_ms
.is_some_and(|submitted_at_ms| received_at_ms >= submitted_at_ms)
&& received_at_ms < active.report.deadline_ms;
if is_match {
self.finish_active(OperationState::Observed, received_at_ms, Some(response));
}
}
fn finish_active(
&mut self,
state: OperationState,
terminal_at_ms: u64,
response: Option<Response>,
) {
let mut active = self.active.take().expect("active operation must exist");
active.report.state = state;
active.report.terminal_at_ms = Some(terminal_at_ms);
active.report.response = response;
self.completed = Some(active.report);
}
fn mark_unknown(&mut self, id: OperationId) {
let terminal_at_ms = self
.active
.as_ref()
.and_then(|active| active.report.dispatching_at_ms)
.unwrap_or(0);
self.mark_unknown_at(id, terminal_at_ms);
}
fn mark_unknown_at(&mut self, id: OperationId, terminal_at_ms: u64) {
if let Some(active) = self.active.as_mut().filter(|active| active.report.id == id) {
active.report.state = OperationState::Unknown;
active.report.terminal_at_ms = Some(terminal_at_ms);
}
}
fn record_submitted(&mut self, id: OperationId, submitted_at_ms: u64) {
if let Some(active) = self.active.as_mut().filter(|active| active.report.id == id) {
active.report.submitted_at_ms = Some(submitted_at_ms);
}
}
fn clock_rollback(&self, now_ms: u64) -> Option<u64> {
self.last_now_ms.filter(|previous_ms| now_ms < *previous_ms)
}
fn commit_clock(&mut self, now_ms: u64) {
self.last_now_ms = Some(now_ms);
}
}
pub struct SendAttempt<'a> {
driver: &'a mut Driver,
id: OperationId,
settled: bool,
}
impl SendAttempt<'_> {
pub fn frame(&self) -> &EncodedFrame {
&self
.driver
.active
.as_ref()
.expect("send attempt owns active operation")
.frame
}
pub fn submitted(mut self, now_ms: u64) -> Result<(), AttemptError> {
if self.driver.clock_rollback(now_ms).is_some() {
self.driver.record_submitted(self.id, now_ms);
self.driver.mark_unknown_at(self.id, now_ms);
self.settled = true;
return Err(AttemptError::ClockRollback);
}
let deadline_ms = self.active_deadline();
if now_ms >= deadline_ms {
self.driver.commit_clock(now_ms);
self.driver.record_submitted(self.id, now_ms);
self.driver.mark_unknown_at(self.id, now_ms);
self.settled = true;
return Err(AttemptError::DeadlineElapsed);
}
self.driver.commit_clock(now_ms);
let command = {
let active = self
.driver
.active
.as_mut()
.expect("send attempt owns active operation");
active.report.state = OperationState::Submitted;
active.report.submitted_at_ms = Some(now_ms);
matches!(active.report.kind, OperationKind::Command(_))
};
if command {
self.driver
.finish_active(OperationState::Submitted, now_ms, None);
}
self.settled = true;
Ok(())
}
pub fn would_block(mut self, now_ms: u64) -> Result<(), AttemptError> {
self.not_sent_inner(now_ms)
}
pub fn not_sent(mut self, now_ms: u64) -> Result<(), AttemptError> {
if self.driver.clock_rollback(now_ms).is_some() {
let terminal_at_ms = self
.driver
.last_now_ms
.expect("begin_send committed the clock");
self.driver
.finish_active(OperationState::Failed, terminal_at_ms, None);
self.settled = true;
return Err(AttemptError::ClockRollback);
}
if now_ms >= self.active_deadline() {
self.driver.commit_clock(now_ms);
self.driver
.finish_active(OperationState::Failed, now_ms, None);
self.settled = true;
return Err(AttemptError::DeadlineElapsed);
}
self.driver.commit_clock(now_ms);
self.driver
.finish_active(OperationState::Failed, now_ms, None);
self.settled = true;
Ok(())
}
fn not_sent_inner(&mut self, now_ms: u64) -> Result<(), AttemptError> {
if self.driver.clock_rollback(now_ms).is_some() {
let active = self
.driver
.active
.as_mut()
.expect("send attempt owns active operation");
active.report.state = OperationState::Prepared;
self.settled = true;
return Err(AttemptError::ClockRollback);
}
if now_ms >= self.active_deadline() {
self.driver.commit_clock(now_ms);
self.driver
.finish_active(OperationState::TimedOut, now_ms, None);
self.settled = true;
return Err(AttemptError::DeadlineElapsed);
}
self.driver.commit_clock(now_ms);
let active = self
.driver
.active
.as_mut()
.expect("send attempt owns active operation");
active.report.state = OperationState::Prepared;
self.settled = true;
Ok(())
}
fn active_deadline(&self) -> u64 {
self.driver
.active
.as_ref()
.expect("send attempt owns active operation")
.report
.deadline_ms
}
}
impl Drop for SendAttempt<'_> {
fn drop(&mut self) {
if !self.settled {
self.driver.mark_unknown(self.id);
}
}
}
fn response_matches(query: Query, response: Response) -> bool {
matches!(
(query, response),
(Query::MotorError, Response::MotorError(_))
| (Query::EncoderError, Response::EncoderError(_))
| (Query::SensorlessError, Response::SensorlessError(_))
| (Query::EncoderEstimates, Response::EncoderEstimates { .. })
| (Query::EncoderCount, Response::EncoderCount { .. })
| (Query::Iq, Response::Iq { .. })
| (
Query::SensorlessEstimates,
Response::SensorlessEstimates { .. }
)
| (Query::VbusVoltage, Response::VbusVoltage(_))
)
}