diff --git a/Cargo.lock b/Cargo.lock index ee0606d4b..9deeef477 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -47,6 +47,56 @@ dependencies = [ "as-slice", ] +[[package]] +name = "anstream" +version = "0.6.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43d5b281e737544384e969a5ccad3f1cdd24b48086a0fc1b2a5262a26b8f4f4a" +dependencies = [ + "anstyle", + "anstyle-parse", + "anstyle-query", + "anstyle-wincon", + "colorchoice", + "is_terminal_polyfill", + "utf8parse", +] + +[[package]] +name = "anstyle" +version = "1.0.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5192cca8006f1fd4f7237516f40fa183bb07f8fbdfedaa0036de5ea9b0b45e78" + +[[package]] +name = "anstyle-parse" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e7644824f0aa2c7b9384579234ef10eb7efb6a0deb83f9630a49594dd9c15c2" +dependencies = [ + "utf8parse", +] + +[[package]] +name = "anstyle-query" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "40c48f72fd53cd289104fc64099abca73db4166ad86ea0b4341abe65af83dadc" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "anstyle-wincon" +version = "3.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "291e6a250ff86cd4a820112fb8898808a366d8f9f58ce16d1f538353ad55747d" +dependencies = [ + "anstyle", + "once_cell_polyfill", + "windows-sys 0.61.2", +] + [[package]] name = "anyhow" version = "1.0.99" @@ -353,6 +403,12 @@ dependencies = [ "num-traits", ] +[[package]] +name = "colorchoice" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b05b61dc5112cbb17e4b6cd61790d9845d13888356391624cbe7e41efeac1e75" + [[package]] name = "const-init" version = "1.0.0" @@ -914,6 +970,29 @@ dependencies = [ "cfg-if", ] +[[package]] +name = "env_filter" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1bf3c259d255ca70051b30e2e95b5446cdb8949ac4cd22c0d7fd634d89f568e2" +dependencies = [ + "log", + "regex", +] + +[[package]] +name = "env_logger" +version = "0.11.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "13c863f0904021b108aa8b2f55046443e6b1ebde8fd4a15c399893aae4fa069f" +dependencies = [ + "anstream", + "anstyle", + "env_filter", + "jiff", + "log", +] + [[package]] name = "equivalent" version = "1.0.2" @@ -1122,6 +1201,12 @@ dependencies = [ "libc", ] +[[package]] +name = "is_terminal_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695" + [[package]] name = "itertools" version = "0.10.5" @@ -1155,6 +1240,30 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4a5f13b858c8d314ee3e8f639011f7ccefe71f97f96e50151fb991f267928e2c" +[[package]] +name = "jiff" +version = "0.2.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e67e8da4c49d6d9909fe03361f9b620f58898859f5c7aded68351e85e71ecf50" +dependencies = [ + "jiff-static", + "log", + "portable-atomic", + "portable-atomic-util", + "serde_core", +] + +[[package]] +name = "jiff-static" +version = "0.2.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e0c84ee7f197eca9a86c6fd6cb771e55eb991632f15f2bc3ca6ec838929e6e78" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.106", +] + [[package]] name = "kdl" version = "6.3.4" @@ -1510,6 +1619,12 @@ version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" +[[package]] +name = "once_cell_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe" + [[package]] name = "partition-manager" version = "0.1.0" @@ -1604,6 +1719,15 @@ version = "1.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f84267b20a16ea918e43c6a88433c2d54fa145c92a811b5b047ccbe153674483" +[[package]] +name = "portable-atomic-util" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d8a2f0d8d040d7848a709caf78912debcc3f33ee4b3cac47d73d1e1069e83507" +dependencies = [ + "portable-atomic", +] + [[package]] name = "power-button-service" version = "0.1.0" @@ -1619,13 +1743,17 @@ dependencies = [ name = "power-policy-service" version = "0.1.0" dependencies = [ + "critical-section", "defmt 0.3.100", "embassy-futures", "embassy-sync", "embassy-time", "embedded-services", + "env_logger", "heapless", "log", + "static_cell", + "tokio", ] [[package]] @@ -1847,18 +1975,28 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" [[package]] name = "serde" -version = "1.0.219" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" dependencies = [ "serde_derive", ] [[package]] name = "serde_derive" -version = "1.0.219" +version = "1.0.228" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" dependencies = [ "proc-macro2", "quote", @@ -2299,6 +2437,12 @@ dependencies = [ "portable-atomic", ] +[[package]] +name = "utf8parse" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821" + [[package]] name = "uuid" version = "1.17.0" @@ -2359,7 +2503,7 @@ dependencies = [ "windows-collections", "windows-core", "windows-future", - "windows-link", + "windows-link 0.1.3", "windows-numerics", ] @@ -2380,7 +2524,7 @@ checksum = "c0fdd3ddb90610c7638aa2b3a3ab2904fb9e5cdbecc643ddb3647212781c4ae3" dependencies = [ "windows-implement", "windows-interface", - "windows-link", + "windows-link 0.1.3", "windows-result", "windows-strings", ] @@ -2392,7 +2536,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fc6a41e98427b19fe4b73c550f060b59fa592d7d686537eebf9385621bfbad8e" dependencies = [ "windows-core", - "windows-link", + "windows-link 0.1.3", "windows-threading", ] @@ -2424,6 +2568,12 @@ version = "0.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5e6ad25900d524eaabdbbb96d20b4311e1e7ae1699af4fb28c17ae66c80d798a" +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + [[package]] name = "windows-numerics" version = "0.2.0" @@ -2431,7 +2581,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9150af68066c4c5c07ddc0ce30421554771e528bde427614c61038bc2c92c2b1" dependencies = [ "windows-core", - "windows-link", + "windows-link 0.1.3", ] [[package]] @@ -2440,7 +2590,7 @@ version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "56f42bd332cc6c8eac5af113fc0c1fd6a8fd2aa08a0119358686e5160d0586c6" dependencies = [ - "windows-link", + "windows-link 0.1.3", ] [[package]] @@ -2449,7 +2599,7 @@ version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "56e6c93f3a0c3b36176cb1327a4958a0353d5d166c2a35cb268ace15e91d3b57" dependencies = [ - "windows-link", + "windows-link 0.1.3", ] [[package]] @@ -2470,6 +2620,15 @@ dependencies = [ "windows-targets", ] +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link 0.2.1", +] + [[package]] name = "windows-targets" version = "0.52.6" @@ -2492,7 +2651,7 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b66463ad2e0ea3bbf808b7f1d371311c80e115c0b71d60efc142cafbcfb057a6" dependencies = [ - "windows-link", + "windows-link 0.1.3", ] [[package]] diff --git a/embedded-service/Cargo.toml b/embedded-service/Cargo.toml index 2570775c5..4199d9ec8 100644 --- a/embedded-service/Cargo.toml +++ b/embedded-service/Cargo.toml @@ -21,6 +21,7 @@ defmt = { workspace = true, optional = true } document-features.workspace = true embassy-sync.workspace = true embassy-time.workspace = true +embassy-futures.workspace = true embedded-batteries-async.workspace = true embedded-cfu-protocol.workspace = true embedded-hal-async.workspace = true @@ -49,7 +50,6 @@ cortex-m.workspace = true [dev-dependencies] critical-section = { workspace = true, features = ["std"] } -embassy-futures.workspace = true embassy-sync = { workspace = true, features = ["std"] } embassy-time = { workspace = true, features = ["std", "generic-queue-8"] } embassy-time-driver = { workspace = true } diff --git a/embedded-service/src/event.rs b/embedded-service/src/event.rs new file mode 100644 index 000000000..20675ae68 --- /dev/null +++ b/embedded-service/src/event.rs @@ -0,0 +1,43 @@ +//! Common traits for event senders and receivers + +use embassy_sync::channel::{DynamicReceiver, DynamicSender}; + +/// Common event sender trait +pub trait Sender { + /// Attempt to send an event + /// + /// Return none if the event cannot currently be sent + fn try_send(&mut self, event: E) -> Option<()>; + /// Send an event + fn send(&mut self, event: E) -> impl Future; +} + +/// Common event receiver trait +pub trait Receiver { + /// Attempt to receive an event + /// + /// Return none if there are no pending events + fn try_next(&mut self) -> Option; + /// Receiver an event + fn wait_next(&mut self) -> impl Future; +} + +impl Sender for DynamicSender<'_, E> { + fn try_send(&mut self, event: E) -> Option<()> { + DynamicSender::try_send(self, event).ok() + } + + fn send(&mut self, event: E) -> impl Future { + DynamicSender::send(self, event) + } +} + +impl Receiver for DynamicReceiver<'_, E> { + fn try_next(&mut self) -> Option { + self.try_receive().ok() + } + + fn wait_next(&mut self) -> impl Future { + self.receive() + } +} diff --git a/embedded-service/src/lib.rs b/embedded-service/src/lib.rs index 7facbbe0d..c38659a8a 100644 --- a/embedded-service/src/lib.rs +++ b/embedded-service/src/lib.rs @@ -17,6 +17,7 @@ pub mod buffer; pub mod cfu; pub mod comms; pub mod ec_type; +pub mod event; pub mod fmt; pub mod hid; pub mod init; diff --git a/embedded-service/src/power/policy/action/device.rs b/embedded-service/src/power/policy/action/device.rs deleted file mode 100644 index 01d0d6c36..000000000 --- a/embedded-service/src/power/policy/action/device.rs +++ /dev/null @@ -1,172 +0,0 @@ -//! Device state machine actions -use super::*; -use crate::power::policy::{ConsumerPowerCapability, Error, ProviderPowerCapability, device, policy}; -use crate::{info, trace}; - -/// Device state machine control -pub struct Device<'a, S: Kind> { - device: &'a device::Device, - _state: core::marker::PhantomData, -} - -/// Enum to contain any state -pub enum AnyState<'a> { - /// Detached - Detached(Device<'a, Detached>), - /// Idle - Idle(Device<'a, Idle>), - /// Connected Consumer - ConnectedConsumer(Device<'a, ConnectedConsumer>), - /// Connected Provider - ConnectedProvider(Device<'a, ConnectedProvider>), -} - -impl AnyState<'_> { - /// Return the kind of the contained state - pub fn kind(&self) -> StateKind { - match self { - AnyState::Detached(_) => StateKind::Detached, - AnyState::Idle(_) => StateKind::Idle, - AnyState::ConnectedConsumer(_) => StateKind::ConnectedConsumer, - AnyState::ConnectedProvider(_) => StateKind::ConnectedProvider, - } - } -} - -impl<'a, S: Kind> Device<'a, S> { - /// Create a new state machine - pub(crate) fn new(device: &'a device::Device) -> Self { - Self { - device, - _state: core::marker::PhantomData, - } - } - - /// Detach the device - pub async fn detach(self) -> Result, Error> { - info!("Received detach from device {}", self.device.id().0); - self.device.set_state(device::State::Detached).await; - self.device.update_consumer_capability(None).await; - self.device.update_requested_provider_capability(None).await; - policy::send_request(self.device.id(), policy::RequestData::NotifyDetached) - .await? - .complete_or_err()?; - Ok(Device::new(self.device)) - } - - /// Disconnect this device - async fn disconnect_internal(&self) -> Result<(), Error> { - info!("Device {} disconnecting", self.device.id().0); - self.device.update_consumer_capability(None).await; - self.device.update_requested_provider_capability(None).await; - self.device.set_state(device::State::Idle).await; - policy::send_request(self.device.id(), policy::RequestData::NotifyDisconnect) - .await? - .complete_or_err() - } - - /// Notify the power policy service of an updated consumer power capability - async fn notify_consumer_power_capability_internal( - &self, - capability: Option, - ) -> Result<(), Error> { - info!( - "Device {} consume capability updated: {:#?}", - self.device.id().0, - capability - ); - self.device.update_consumer_capability(capability).await; - policy::send_request( - self.device.id(), - policy::RequestData::NotifyConsumerCapability(capability), - ) - .await? - .complete_or_err() - } - - /// Request the given power from the power policy service - async fn request_provider_power_capability_internal( - &self, - capability: ProviderPowerCapability, - ) -> Result<(), Error> { - if self.device.provider_capability().await == Some(capability) { - // Already operating at this capability, power policy is already aware, don't need to do anything - trace!("Device {} already requested: {:#?}", self.device.id().0, capability); - return Ok(()); - } - - info!("Request provide from device {}, {:#?}", self.device.id().0, capability); - self.device.update_requested_provider_capability(Some(capability)).await; - policy::send_request( - self.device.id(), - policy::RequestData::RequestProviderCapability(capability), - ) - .await? - .complete_or_err()?; - Ok(()) - } -} - -impl<'a> Device<'a, Detached> { - /// Attach the device - pub async fn attach(self) -> Result, Error> { - info!("Received attach from device {}", self.device.id().0); - self.device.set_state(device::State::Idle).await; - policy::send_request(self.device.id(), policy::RequestData::NotifyAttached) - .await? - .complete_or_err()?; - Ok(Device::new(self.device)) - } -} - -impl Device<'_, Idle> { - /// Notify the power policy service of an updated consumer power capability - pub async fn notify_consumer_power_capability( - &self, - capability: Option, - ) -> Result<(), Error> { - self.notify_consumer_power_capability_internal(capability).await - } - - /// Request the given power from the power policy service - pub async fn request_provider_power_capability(&self, capability: ProviderPowerCapability) -> Result<(), Error> { - self.request_provider_power_capability_internal(capability).await - } -} - -impl<'a> Device<'a, ConnectedConsumer> { - /// Disconnect this device - pub async fn disconnect(self) -> Result, Error> { - self.disconnect_internal().await?; - Ok(Device::new(self.device)) - } - - /// Notify the power policy service of an updated consumer power capability - pub async fn notify_consumer_power_capability( - &self, - capability: Option, - ) -> Result<(), Error> { - self.notify_consumer_power_capability_internal(capability).await - } -} - -impl<'a> Device<'a, ConnectedProvider> { - /// Disconnect this device - pub async fn disconnect(self) -> Result, Error> { - self.disconnect_internal().await?; - Ok(Device::new(self.device)) - } - - /// Request the given power from the power policy service - pub async fn request_provider_power_capability(&self, capability: ProviderPowerCapability) -> Result<(), Error> { - self.request_provider_power_capability_internal(capability).await - } - - /// Notify the power policy service of an updated consumer power capability - pub async fn notify_consumer_power_capability( - &self, - capability: Option, - ) -> Result<(), Error> { - self.notify_consumer_power_capability_internal(capability).await - } -} diff --git a/embedded-service/src/power/policy/action/mod.rs b/embedded-service/src/power/policy/action/mod.rs deleted file mode 100644 index 889dccf70..000000000 --- a/embedded-service/src/power/policy/action/mod.rs +++ /dev/null @@ -1,51 +0,0 @@ -//! Power policy actions -//! This modules contains wrapper structs that use type states to enforce the valid actions for each device state -use super::device::StateKind; - -pub mod device; -pub mod policy; - -trait Sealed {} - -/// Trait to provide the kind of a state type -#[allow(private_bounds)] -pub trait Kind: Sealed { - /// Return the kind of a state type - fn kind() -> StateKind; -} - -/// State type for a detached device -pub struct Detached; -impl Sealed for Detached {} -impl Kind for Detached { - fn kind() -> StateKind { - StateKind::Detached - } -} - -/// State type for an attached device -pub struct Idle; -impl Sealed for Idle {} -impl Kind for Idle { - fn kind() -> StateKind { - StateKind::Idle - } -} - -/// State type for a device that is providing power -pub struct ConnectedProvider; -impl Sealed for ConnectedProvider {} -impl Kind for ConnectedProvider { - fn kind() -> StateKind { - StateKind::ConnectedProvider - } -} - -/// State type for a device that is consuming power -pub struct ConnectedConsumer; -impl Sealed for ConnectedConsumer {} -impl Kind for ConnectedConsumer { - fn kind() -> StateKind { - StateKind::ConnectedConsumer - } -} diff --git a/embedded-service/src/power/policy/action/policy.rs b/embedded-service/src/power/policy/action/policy.rs deleted file mode 100644 index 0559421fc..000000000 --- a/embedded-service/src/power/policy/action/policy.rs +++ /dev/null @@ -1,238 +0,0 @@ -//! Policy state machine -use embassy_time::{Duration, TimeoutError, with_timeout}; - -use super::*; -use crate::power::policy::{ConsumerPowerCapability, Error, ProviderPowerCapability, device}; -use crate::{error, info}; - -/// Default timeout for device commands to prevent the policy from getting stuck -const DEFAULT_TIMEOUT: Duration = Duration::from_millis(5000); - -/// Policy state machine control -pub struct Policy<'a, S: Kind> { - device: &'a device::Device, - _state: core::marker::PhantomData, -} - -/// Enum to contain any state -pub enum AnyState<'a> { - /// Detached - Detached(Policy<'a, Detached>), - /// Idle - Idle(Policy<'a, Idle>), - /// Connected Consumer - ConnectedConsumer(Policy<'a, ConnectedConsumer>), - /// Connected Provider - ConnectedProvider(Policy<'a, ConnectedProvider>), -} - -impl AnyState<'_> { - /// Return the kind of the contained state - pub fn kind(&self) -> StateKind { - match self { - AnyState::Detached(_) => StateKind::Detached, - AnyState::Idle(_) => StateKind::Idle, - AnyState::ConnectedConsumer(_) => StateKind::ConnectedConsumer, - AnyState::ConnectedProvider(_) => StateKind::ConnectedProvider, - } - } -} - -impl<'a, S: Kind> Policy<'a, S> { - /// Create a new state machine - pub(crate) fn new(device: &'a device::Device) -> Self { - Self { - device, - _state: core::marker::PhantomData, - } - } - - /// Common disconnect function used by multiple states - async fn disconnect_internal_no_timeout(&self) -> Result<(), Error> { - info!("Device {} got disconnect request", self.device.id().0); - self.device - .execute_device_command(device::CommandData::Disconnect) - .await? - .complete_or_err()?; - self.device.set_state(device::State::Idle).await; - Ok(()) - } - - /// Common disconnect function used by multiple states - async fn disconnect_internal(&self) -> Result<(), Error> { - match with_timeout(DEFAULT_TIMEOUT, self.disconnect_internal_no_timeout()).await { - Ok(r) => r, - Err(TimeoutError) => Err(Error::Timeout), - } - } - - /// Common connect as provider function used by multiple states - async fn connect_as_provider_internal_no_timeout(&self, capability: ProviderPowerCapability) -> Result<(), Error> { - info!("Device {} connecting provider", self.device.id().0); - - self.device - .execute_device_command(device::CommandData::ConnectAsProvider(capability)) - .await? - .complete_or_err()?; - - self.device - .set_state(device::State::ConnectedProvider(capability)) - .await; - - Ok(()) - } - - /// Common connect provider function used by multiple states - async fn connect_provider_internal(&self, capability: ProviderPowerCapability) -> Result<(), Error> { - match with_timeout( - DEFAULT_TIMEOUT, - self.connect_as_provider_internal_no_timeout(capability), - ) - .await - { - Ok(r) => r, - Err(TimeoutError) => Err(Error::Timeout), - } - } -} - -// The policy can do nothing when no device is attached -impl Policy<'_, Detached> {} - -impl<'a> Policy<'a, Idle> { - /// Connect this device as a consumer - pub async fn connect_as_consumer_no_timeout( - self, - capability: ConsumerPowerCapability, - ) -> Result, Error> { - info!("Device {} connecting as consumer", self.device.id().0); - - self.device - .execute_device_command(device::CommandData::ConnectAsConsumer(capability)) - .await? - .complete_or_err()?; - - self.device - .set_state(device::State::ConnectedConsumer(capability)) - .await; - Ok(Policy::new(self.device)) - } - - /// Connect this device as a consumer - pub async fn connect_consumer( - self, - capability: ConsumerPowerCapability, - ) -> Result, Error> { - match with_timeout(DEFAULT_TIMEOUT, self.connect_as_consumer_no_timeout(capability)).await { - Ok(r) => r, - Err(TimeoutError) => Err(Error::Timeout), - } - } - - /// Connect this device as a provider - pub async fn connect_provider_no_timeout( - self, - capability: ProviderPowerCapability, - ) -> Result, Error> { - self.connect_as_provider_internal_no_timeout(capability) - .await - .map(|_| Policy::new(self.device)) - } - - /// Connect this device as a provider - pub async fn connect_provider( - self, - capability: ProviderPowerCapability, - ) -> Result, Error> { - self.connect_provider_internal(capability) - .await - .map(|_| Policy::new(self.device)) - } -} - -impl<'a> Policy<'a, ConnectedConsumer> { - /// Disconnect this device - pub async fn disconnect_no_timeout(self) -> Result, Error> { - self.disconnect_internal_no_timeout() - .await - .map(|_| Policy::new(self.device)) - } - - /// Disconnect this device - pub async fn disconnect(self) -> Result, Error> { - self.disconnect_internal().await.map(|_| Policy::new(self.device)) - } -} - -impl<'a> Policy<'a, ConnectedProvider> { - /// Disconnect this device - pub async fn disconnect_no_timeout(self) -> Result, Error> { - if let Err(e) = self.disconnect_internal_no_timeout().await { - error!("Error disconnecting device {}: {:?}", self.device.id().0, e); - return Err(e); - } - Ok(Policy::new(self.device)) - } - - /// Disconnect this device - pub async fn disconnect(self) -> Result, Error> { - match with_timeout(DEFAULT_TIMEOUT, self.disconnect_no_timeout()).await { - Ok(r) => r, - Err(TimeoutError) => Err(Error::Timeout), - } - } - - /// Connect this device as a consumer - pub async fn connect_as_consumer_no_timeout( - self, - capability: ConsumerPowerCapability, - ) -> Result, Error> { - info!("Device {} connecting as consumer", self.device.id().0); - - self.device - .execute_device_command(device::CommandData::ConnectAsConsumer(capability)) - .await? - .complete_or_err()?; - - self.device - .set_state(device::State::ConnectedConsumer(capability)) - .await; - Ok(Policy::new(self.device)) - } - - /// Connect this device as a consumer - pub async fn connect_consumer( - self, - capability: ConsumerPowerCapability, - ) -> Result, Error> { - match with_timeout(DEFAULT_TIMEOUT, self.connect_as_consumer_no_timeout(capability)).await { - Ok(r) => r, - Err(TimeoutError) => Err(Error::Timeout), - } - } - - /// Connect this device as a provider - pub async fn connect_provider_no_timeout( - &self, - capability: ProviderPowerCapability, - ) -> Result, Error> { - self.connect_as_provider_internal_no_timeout(capability) - .await - .map(|_| Policy::new(self.device)) - } - - /// Connect this device as a provider - pub async fn connect_provider( - &self, - capability: ProviderPowerCapability, - ) -> Result, Error> { - self.connect_provider_internal(capability) - .await - .map(|_| Policy::new(self.device)) - } - - /// Get the provider power capability of this device - pub async fn power_capability(&self) -> Option { - self.device.provider_capability().await - } -} diff --git a/embedded-service/src/power/policy/device.rs b/embedded-service/src/power/policy/device.rs index 23a6cc052..22c3a501f 100644 --- a/embedded-service/src/power/policy/device.rs +++ b/embedded-service/src/power/policy/device.rs @@ -1,11 +1,11 @@ //! Device struct and methods -use core::ops::DerefMut; - use embassy_sync::mutex::Mutex; -use super::{DeviceId, Error, action}; -use crate::ipc::deferred; +use super::{DeviceId, Error}; +use crate::event::Receiver; +use crate::power::policy::policy::RequestData; use crate::power::policy::{ConsumerPowerCapability, ProviderPowerCapability}; +use crate::sync::Lockable; use crate::{GlobalRawMutex, intrusive_list}; /// Most basic device states @@ -48,16 +48,159 @@ impl State { } } -/// Internal device state for power policy +/// Per-device state for power policy implementation +/// +/// This struct implements the state machine outlined in the docs directory. +/// The various state transition functions always succeed in the sense that +/// the desired state is always entered, but some still return a result. +/// This is because a the device that is driving this state machine is the +/// ultimate source of truth and the recovery procedure would ultimately +/// end up catching up to this state anyway. #[derive(Debug, Clone, Copy, PartialEq, Eq)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -struct InternalState { +pub struct InternalState { /// Current state of the device - pub state: State, + state: State, /// Current consumer capability - pub consumer_capability: Option, + consumer_capability: Option, /// Current requested provider capability - pub requested_provider_capability: Option, + requested_provider_capability: Option, +} + +impl Default for InternalState { + fn default() -> Self { + Self { + state: State::Detached, + consumer_capability: None, + requested_provider_capability: None, + } + } +} + +impl InternalState { + /// Attach the device + pub fn attach(&mut self) -> Result<(), Error> { + let result = if self.state == State::Detached { + Ok(()) + } else { + Err(Error::InvalidState(&[StateKind::Detached], self.state.kind())) + }; + self.state = State::Idle; + result + } + + /// Detach the device + /// + /// Detach is always a valid transition + pub fn detach(&mut self) { + self.state = State::Detached; + self.consumer_capability = None; + self.requested_provider_capability = None; + } + + /// Disconnect this device + pub fn disconnect(&mut self, clear_caps: bool) -> Result<(), Error> { + let result = if matches!(self.state, State::ConnectedConsumer(_) | State::ConnectedProvider(_)) { + Ok(()) + } else { + Err(Error::InvalidState( + &[StateKind::ConnectedConsumer, StateKind::ConnectedProvider], + self.state.kind(), + )) + }; + self.state = State::Idle; + if clear_caps { + self.consumer_capability = None; + self.requested_provider_capability = None; + } + result + } + + /// Update the available consumer capability + pub fn update_consumer_power_capability( + &mut self, + capability: Option, + ) -> Result<(), Error> { + let result = if matches!( + self.state, + State::Idle | State::ConnectedConsumer(_) | State::ConnectedProvider(_) + ) { + Ok(()) + } else { + Err(Error::InvalidState( + &[StateKind::Idle, StateKind::ConnectedConsumer], + self.state.kind(), + )) + }; + self.consumer_capability = capability; + result + } + + /// Updated the requested provider capability + pub fn update_requested_provider_power_capability( + &mut self, + capability: Option, + ) -> Result<(), Error> { + if self.requested_provider_capability == capability { + // Already operating at this capability, power policy is already aware, don't need to do anything + return Ok(()); + } + + let result = if matches!( + self.state, + State::Idle | State::ConnectedConsumer(_) | State::ConnectedProvider(_) + ) { + Ok(()) + } else { + Err(Error::InvalidState( + &[StateKind::Idle, StateKind::ConnectedProvider], + self.state.kind(), + )) + }; + + self.requested_provider_capability = capability; + result + } + + /// Handle a request to connect as a consumer from the policy + pub fn connect_consumer(&mut self, capability: ConsumerPowerCapability) -> Result<(), Error> { + let result = if self.state == State::Idle { + Ok(()) + } else { + Err(Error::InvalidState(&[StateKind::Idle], self.state.kind())) + }; + self.state = State::ConnectedConsumer(capability); + result + } + + /// Handle a request to connect as a provider from the policy + pub fn connect_provider(&mut self, capability: ProviderPowerCapability) -> Result<(), Error> { + let result = if matches!(self.state, State::Idle | State::ConnectedProvider(_)) { + Ok(()) + } else { + Err(Error::InvalidState( + &[StateKind::Idle, StateKind::ConnectedProvider], + self.state.kind(), + )) + }; + self.state = State::ConnectedProvider(capability); + result + } + + /// Returns the current state machine state + pub fn state(&self) -> State { + self.state + } + + /// Returns the current consumer capability + pub fn consumer_capability(&self) -> Option { + self.consumer_capability + } + + /// Returns the requested provider capability + pub fn requested_provider_capability(&self) -> Option { + self.requested_provider_capability + } } /// Data for a device request @@ -110,21 +253,39 @@ pub struct Response { pub data: ResponseData, } +/// Trait for devices that can be controlled by a power policy implementation +pub trait DeviceTrait { + /// Disconnect power from this device + fn disconnect(&mut self) -> impl Future>; + /// Connect this device to provide power to an external connection + fn connect_provider(&mut self, capability: ProviderPowerCapability) -> impl Future>; + /// Connect this device to consume power from an external connection + fn connect_consumer(&mut self, capability: ConsumerPowerCapability) -> impl Future>; +} + /// Device struct -pub struct Device { +pub struct Device<'a, D: Lockable, R: Receiver> +where + D::Inner: DeviceTrait, +{ /// Intrusive list node node: intrusive_list::Node, /// Device ID id: DeviceId, /// Current state of the device - state: Mutex, - /// Command channel - command: deferred::Channel, + pub state: Mutex, + /// Reference to hardware + pub device: &'a D, + /// Event receiver + pub receiver: Mutex, } -impl Device { +impl<'a, D: Lockable, R: Receiver> Device<'a, D, R> +where + D::Inner: DeviceTrait, +{ /// Create a new device - pub fn new(id: DeviceId) -> Self { + pub fn new(id: DeviceId, device: &'a D, receiver: R) -> Self { Self { node: intrusive_list::Node::uninit(), id, @@ -133,7 +294,8 @@ impl Device { consumer_capability: None, requested_provider_capability: None, }), - command: deferred::Channel::new(), + device, + receiver: Mutex::new(receiver), } } @@ -142,11 +304,6 @@ impl Device { self.id } - /// Returns the current state of the device - pub async fn state(&self) -> State { - self.state.lock().await.state - } - /// Returns the current consumer capability of the device pub async fn consumer_capability(&self) -> Option { self.state.lock().await.consumer_capability @@ -154,12 +311,12 @@ impl Device { /// Returns true if the device is currently consuming power pub async fn is_consumer(&self) -> bool { - self.state().await.kind() == StateKind::ConnectedConsumer + self.state.lock().await.state.kind() == StateKind::ConnectedConsumer } /// Returns current provider power capability pub async fn provider_capability(&self) -> Option { - match self.state().await { + match self.state.lock().await.state { State::ConnectedProvider(capability) => Some(capability), _ => None, } @@ -172,115 +329,33 @@ impl Device { /// Returns true if the device is currently providing power pub async fn is_provider(&self) -> bool { - self.state().await.kind() == StateKind::ConnectedProvider - } - - /// Execute a command on the device - pub(super) async fn execute_device_command(&self, command: CommandData) -> Result { - self.command.execute(command).await - } - - /// Create a handler for the command channel - /// - /// DROP SAFETY: Direct call to deferred channel primitive - pub async fn receive(&self) -> deferred::Request<'_, GlobalRawMutex, CommandData, InternalResponseData> { - self.command.receive().await - } - - /// Internal function to set device state - pub(super) async fn set_state(&self, new_state: State) { - let mut lock = self.state.lock().await; - let state = lock.deref_mut(); - state.state = new_state; - } - - /// Internal function to set consumer capability - pub(super) async fn update_consumer_capability(&self, capability: Option) { - let mut lock = self.state.lock().await; - let state = lock.deref_mut(); - state.consumer_capability = capability; - } - - /// Internal function to set requested provider capability - pub(super) async fn update_requested_provider_capability(&self, capability: Option) { - let mut lock = self.state.lock().await; - let state = lock.deref_mut(); - state.requested_provider_capability = capability; - } - - /// Try to provide access to the device actions for the given state - pub async fn try_device_action(&self) -> Result, Error> { - let state = self.state().await.kind(); - if S::kind() != state { - return Err(Error::InvalidState(S::kind(), state)); - } - Ok(action::device::Device::new(self)) - } - - /// Provide access to the current device state - pub async fn device_action(&self) -> action::device::AnyState<'_> { - match self.state().await.kind() { - StateKind::Detached => action::device::AnyState::Detached(action::device::Device::new(self)), - StateKind::Idle => action::device::AnyState::Idle(action::device::Device::new(self)), - StateKind::ConnectedProvider => { - action::device::AnyState::ConnectedProvider(action::device::Device::new(self)) - } - StateKind::ConnectedConsumer => { - action::device::AnyState::ConnectedConsumer(action::device::Device::new(self)) - } - } - } - - /// Try to provide access to the policy actions for the given state - /// Implemented here for lifetime reasons - pub(super) async fn try_policy_action(&self) -> Result, Error> { - let state = self.state().await.kind(); - if S::kind() != state { - return Err(Error::InvalidState(S::kind(), state)); - } - Ok(action::policy::Policy::new(self)) - } - - /// Provide access to the current policy actions - /// Implemented here for lifetime reasons - pub(super) async fn policy_action(&self) -> action::policy::AnyState<'_> { - match self.state().await.kind() { - StateKind::Detached => action::policy::AnyState::Detached(action::policy::Policy::new(self)), - StateKind::Idle => action::policy::AnyState::Idle(action::policy::Policy::new(self)), - StateKind::ConnectedProvider => { - action::policy::AnyState::ConnectedProvider(action::policy::Policy::new(self)) - } - StateKind::ConnectedConsumer => { - action::policy::AnyState::ConnectedConsumer(action::policy::Policy::new(self)) - } - } - } - - /// Detach the device, this action is available in all states - pub async fn detach(&self) -> Result, Error> { - match self.device_action().await { - action::device::AnyState::Detached(state) => Ok(state), - action::device::AnyState::Idle(state) => state.detach().await, - action::device::AnyState::ConnectedProvider(state) => state.detach().await, - action::device::AnyState::ConnectedConsumer(state) => state.detach().await, - } + self.state.lock().await.state.kind() == StateKind::ConnectedProvider } } -impl intrusive_list::NodeContainer for Device { +impl + 'static> intrusive_list::NodeContainer for Device<'static, D, R> +where + D::Inner: DeviceTrait, +{ fn get_node(&self) -> &crate::Node { &self.node } } /// Trait for any container that holds a device -pub trait DeviceContainer { +pub trait DeviceContainer> +where + D::Inner: DeviceTrait, +{ /// Get the underlying device struct - fn get_power_policy_device(&self) -> &Device; + fn get_power_policy_device(&self) -> &Device<'_, D, R>; } -impl DeviceContainer for Device { - fn get_power_policy_device(&self) -> &Device { +impl> DeviceContainer for Device<'_, D, R> +where + D::Inner: DeviceTrait, +{ + fn get_power_policy_device(&self) -> &Device<'_, D, R> { self } } diff --git a/embedded-service/src/power/policy/mod.rs b/embedded-service/src/power/policy/mod.rs index b925f860b..d33efc63f 100644 --- a/embedded-service/src/power/policy/mod.rs +++ b/embedded-service/src/power/policy/mod.rs @@ -1,5 +1,4 @@ //! Power policy related data structures and messages -pub mod action; pub mod charger; pub mod device; pub mod flags; @@ -20,7 +19,7 @@ pub enum Error { /// The consume request was denied, contains maximum available power CannotConsume(Option), /// The device is not in the correct state (expected, actual) - InvalidState(device::StateKind, device::StateKind), + InvalidState(&'static [device::StateKind], device::StateKind), /// Invalid response InvalidResponse, /// Busy, the device cannot respond to the request at this time diff --git a/embedded-service/src/power/policy/policy.rs b/embedded-service/src/power/policy/policy.rs index 3e3d694bb..3e931c4bb 100644 --- a/embedded-service/src/power/policy/policy.rs +++ b/embedded-service/src/power/policy/policy.rs @@ -1,34 +1,35 @@ //! Context for any power policy implementations +use core::marker::PhantomData; +use core::pin::pin; use core::sync::atomic::{AtomicBool, Ordering}; -use crate::GlobalRawMutex; use crate::broadcaster::immediate as broadcaster; +use crate::event::Receiver; +use crate::power::policy::device::DeviceTrait; use crate::power::policy::{CommsMessage, ConsumerPowerCapability, ProviderPowerCapability}; -use embassy_sync::channel::Channel; +use crate::sync::Lockable; +use embassy_futures::select::select_slice; use super::charger::ChargerResponse; use super::device::{self}; -use super::{DeviceId, Error, action, charger}; +use super::{DeviceId, Error, charger}; use crate::power::policy::charger::ChargerResponseData::Ack; use crate::{error, intrusive_list}; -/// Number of slots for policy requests -const POLICY_CHANNEL_SIZE: usize = 1; - /// Data for a power policy request #[derive(Debug, Clone, Copy, PartialEq, Eq)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum RequestData { /// Notify that a device has attached - NotifyAttached, + Attached, /// Notify that available power for consumption has changed - NotifyConsumerCapability(Option), + UpdatedConsumerCapability(Option), /// Request the given amount of power to provider - RequestProviderCapability(ProviderPowerCapability), + RequestedProviderCapability(Option), /// Notify that a device cannot consume or provide power anymore - NotifyDisconnect, + Disconnected, /// Notify that a device has detached - NotifyDetached, + Detached, } /// Request to the power policy service @@ -68,17 +69,10 @@ pub struct Response { pub data: ResponseData, } -/// Wrapper type to make code cleaner -type InternalResponseData = Result; - /// Power policy context struct Context { /// Registered devices devices: intrusive_list::IntrusiveList, - /// Policy request - policy_request: Channel, - /// Policy response - policy_response: Channel, /// Registered chargers chargers: intrusive_list::IntrusiveList, /// Message broadcaster @@ -90,8 +84,6 @@ impl Context { Self { devices: intrusive_list::IntrusiveList::new(), chargers: intrusive_list::IntrusiveList::new(), - policy_request: Channel::new(), - policy_response: Channel::new(), broadcaster: broadcaster::Immediate::new(), } } @@ -103,9 +95,14 @@ static CONTEXT: Context = Context::new(); pub fn init() {} /// Register a device with the power policy service -pub fn register_device(device: &'static impl device::DeviceContainer) -> Result<(), intrusive_list::Error> { +pub fn register_device + 'static>( + device: &'static impl device::DeviceContainer, +) -> Result<(), intrusive_list::Error> +where + D::Inner: DeviceTrait, +{ let device = device.get_power_policy_device(); - if get_device(device.id()).is_some() { + if get_device::(device.id()).is_some() { return Err(intrusive_list::Error::NodeAlreadyInList); } @@ -123,9 +120,14 @@ pub fn register_charger(device: &'static impl charger::ChargerContainer) -> Resu } /// Find a device by its ID -fn get_device(id: DeviceId) -> Option<&'static device::Device> { +fn get_device + 'static>( + id: DeviceId, +) -> Option<&'static device::Device<'static, D, R>> +where + D::Inner: DeviceTrait, +{ for device in &CONTEXT.devices { - if let Some(data) = device.data::() { + if let Some(data) = device.data::>() { if data.id() == id { return Some(data); } @@ -138,9 +140,12 @@ fn get_device(id: DeviceId) -> Option<&'static device::Device> { } /// Returns the total amount of power that is being supplied to external devices -pub async fn compute_total_provider_power_mw() -> u32 { +pub async fn compute_total_provider_power_mw + 'static>() -> u32 +where + D::Inner: DeviceTrait, +{ let mut total = 0; - for device in CONTEXT.devices.iter_only::() { + for device in CONTEXT.devices.iter_only::>() { if let Some(capability) = device.provider_capability().await { if device.is_provider().await { total += capability.capability.max_power_mw(); @@ -165,18 +170,6 @@ fn get_charger(id: charger::ChargerId) -> Option<&'static charger::Device> { None } -/// Convenience function to send a request to the power policy service -pub(super) async fn send_request(from: DeviceId, request: RequestData) -> Result { - CONTEXT - .policy_request - .send(Request { - id: from, - data: request, - }) - .await; - CONTEXT.policy_response.receive().await -} - /// Initialize chargers in hardware pub async fn init_chargers() -> ChargerResponse { for charger in &CONTEXT.chargers { @@ -209,9 +202,17 @@ pub fn register_message_receiver( } /// Singleton struct to give access to the power policy context -pub struct ContextToken(()); +pub struct ContextToken> +where + D::Inner: DeviceTrait, +{ + _phantom: PhantomData<(D, R)>, +} -impl ContextToken { +impl + 'static> ContextToken +where + D::Inner: DeviceTrait, +{ /// Create a new context token, returning None if this function has been called before pub fn create() -> Option { static INIT: AtomicBool = AtomicBool::new(false); @@ -220,7 +221,7 @@ impl ContextToken { } INIT.store(true, Ordering::SeqCst); - Some(ContextToken(())) + Some(ContextToken { _phantom: PhantomData }) } /// Initialize Policy charger devices @@ -233,18 +234,8 @@ impl ContextToken { Ok(()) } - /// Wait for a power policy request - pub async fn wait_request(&self) -> Request { - CONTEXT.policy_request.receive().await - } - - /// Send a response to a power policy request - pub async fn send_response(&self, response: Result) { - CONTEXT.policy_response.send(response).await - } - /// Get a device by its ID - pub fn get_device(&self, id: DeviceId) -> Result<&'static device::Device, Error> { + pub fn get_device(&self, id: DeviceId) -> Result<&'static device::Device<'static, D, R>, Error> { get_device(id).ok_or(Error::InvalidDevice) } @@ -263,21 +254,30 @@ impl ContextToken { &CONTEXT.chargers } - /// Try to provide access to the actions available to the policy for the given state and device - pub async fn try_policy_action( - &self, - id: DeviceId, - ) -> Result, Error> { - self.get_device(id)?.try_policy_action().await - } - - /// Provide access to current policy actions - pub async fn policy_action(&self, id: DeviceId) -> Result, Error> { - Ok(self.get_device(id)?.policy_action().await) - } - /// Broadcast a power policy message to all subscribers pub async fn broadcast_message(&self, message: CommsMessage) { CONTEXT.broadcaster.broadcast(message).await; } + + /// Get the next pending device event + pub async fn wait_request(&self) -> Request { + let mut futures = heapless::Vec::<_, 16>::new(); + for device in self.devices().iter_only::>() { + // TODO: check this at compile time + let _ = futures.push(async { device.receiver.lock().await.wait_next().await }); + } + + let (event, index) = select_slice(pin!(&mut futures)).await; + // Panic safety: The index is guaranteed to be within bounds since it comes from the select_slice result + #[allow(clippy::unwrap_used)] + let device = self + .devices() + .iter_only::>() + .nth(index) + .unwrap(); + Request { + id: device.id(), + data: event, + } + } } diff --git a/examples/rt633/Cargo.lock b/examples/rt633/Cargo.lock index c1215bae5..f93d05d0c 100644 --- a/examples/rt633/Cargo.lock +++ b/examples/rt633/Cargo.lock @@ -736,6 +736,7 @@ dependencies = [ "critical-section", "defmt 0.3.100", "document-features", + "embassy-futures", "embassy-sync", "embassy-time", "embedded-batteries-async", diff --git a/examples/rt685s-evk/Cargo.lock b/examples/rt685s-evk/Cargo.lock index 52cc130ee..a6ee8f96e 100644 --- a/examples/rt685s-evk/Cargo.lock +++ b/examples/rt685s-evk/Cargo.lock @@ -706,6 +706,7 @@ dependencies = [ "critical-section", "defmt 0.3.100", "document-features", + "embassy-futures", "embassy-sync", "embassy-time", "embedded-batteries-async", diff --git a/examples/rt685s-evk/src/bin/type_c.rs b/examples/rt685s-evk/src/bin/type_c.rs index 3d46d274d..e7a57ccca 100644 --- a/examples/rt685s-evk/src/bin/type_c.rs +++ b/examples/rt685s-evk/src/bin/type_c.rs @@ -8,21 +8,24 @@ use embassy_imxrt::gpio::{Input, Inverter, Pull}; use embassy_imxrt::i2c::Async; use embassy_imxrt::i2c::master::{Config, I2cMaster}; use embassy_imxrt::{bind_interrupts, peripherals}; +use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender}; use embassy_sync::mutex::Mutex; use embassy_sync::pubsub::PubSubChannel; -use embassy_time::{self as _, Delay}; +use embassy_time::{self as _, Delay, Timer}; use embedded_cfu_protocol::protocol_definitions::{FwUpdateOffer, FwUpdateOfferResponse, FwVersion, HostToken}; -use embedded_services::power::policy::{CommsMessage, DeviceId as PowerId}; +use embedded_services::power::policy::{CommsMessage, DeviceId as PowerId, policy}; use embedded_services::type_c::{Cached, ControllerId}; use embedded_services::{GlobalRawMutex, IntrusiveList}; use embedded_services::{error, info}; use embedded_usb_pd::GlobalPortId; +use power_policy_service::PowerPolicy; use static_cell::StaticCell; use tps6699x::asynchronous::embassy as tps6699x; use type_c_service::driver::tps6699x::{self as tps6699x_drv}; use type_c_service::service::Service; use type_c_service::wrapper::ControllerWrapper; -use type_c_service::wrapper::backing::{ReferencedStorage, Storage}; +use type_c_service::wrapper::backing::{IntermediateStorage, ReferencedStorage, Storage}; +use type_c_service::wrapper::proxy::PowerProxyDevice; extern crate rt685s_evk_example; @@ -49,7 +52,14 @@ impl type_c_service::wrapper::FwOfferValidator for Validator { type BusMaster<'a> = I2cMaster<'a, Async>; type BusDevice<'a> = I2cDevice<'a, GlobalRawMutex, BusMaster<'a>>; type Tps6699xMutex<'a> = Mutex>>; -type Wrapper<'a> = ControllerWrapper<'a, GlobalRawMutex, Tps6699xMutex<'a>, Validator>; +type Wrapper<'a> = ControllerWrapper< + 'a, + GlobalRawMutex, + Tps6699xMutex<'a>, + DynamicSender<'a, policy::RequestData>, + DynamicReceiver<'a, policy::RequestData>, + Validator, +>; type Controller<'a> = tps6699x::controller::Controller>; type Interrupt<'a> = tps6699x::Interrupt<'a, GlobalRawMutex, BusDevice<'a>>; @@ -69,7 +79,15 @@ async fn interrupt_task(mut int_in: Input<'static>, mut interrupt: Interrupt<'st #[embassy_executor::task] async fn power_policy_service_task() { - power_policy_service::task::task(Default::default()) + static POWER_POLICY: static_cell::StaticCell< + PowerPolicy>, DynamicReceiver<'static, policy::RequestData>>, + > = static_cell::StaticCell::new(); + let power_policy = + POWER_POLICY.init(PowerPolicy::create(Default::default()).expect("Failed to create power policy")); + + // TODO: remove once power policy task accepts context + Timer::after_millis(100).await; + power_policy_service::task::task(power_policy) .await .expect("Failed to start power policy service task"); } @@ -80,8 +98,6 @@ async fn service_task( controllers: &'static IntrusiveList, wrappers: [&'static Wrapper<'static>; NUM_PD_CONTROLLERS], ) { - info!("Starting type-c task"); - // The service is the only receiver and we only use a DynImmediatePublisher, which doesn't take a publisher slot static POWER_POLICY_CHANNEL: StaticCell> = StaticCell::new(); @@ -111,9 +127,6 @@ async fn main(spawner: Spawner) { info!("Embedded service init"); embedded_services::init().await; - info!("Spawining power policy task"); - spawner.must_spawn(power_policy_service_task()); - static CONTROLLER_LIST: StaticCell = StaticCell::new(); let controllers = CONTROLLER_LIST.init(IntrusiveList::new()); static CONTEXT: StaticCell = StaticCell::new(); @@ -157,13 +170,40 @@ async fn main(spawner: Spawner) { controller_context, CONTROLLER0_ID, 0, // CFU component ID - [(PORT0_ID, PORT0_PWR_ID), (PORT1_ID, PORT1_PWR_ID)], + [PORT0_ID, PORT1_ID], )); - static REFERENCED: StaticCell> = StaticCell::new(); - let referenced = REFERENCED.init( + static INTERMEDIATE: StaticCell> = StaticCell::new(); + let intermediate = INTERMEDIATE.init( storage - .create_referenced() + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL0: StaticCell> = StaticCell::new(); + let policy_channel0 = POLICY_CHANNEL0.init(Channel::new()); + let policy_sender0 = policy_channel0.dyn_sender(); + let policy_receiver0 = policy_channel0.dyn_receiver(); + + static POLICY_CHANNEL1: StaticCell> = StaticCell::new(); + let policy_channel1 = POLICY_CHANNEL1.init(Channel::new()); + let policy_sender1 = policy_channel1.dyn_sender(); + let policy_receiver1 = policy_channel1.dyn_receiver(); + + static REFERENCED: StaticCell< + ReferencedStorage< + TPS66994_NUM_PORTS, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced = REFERENCED.init( + intermediate + .try_create_referenced([ + (PORT0_PWR_ID, policy_sender0, policy_receiver0), + (PORT1_PWR_ID, policy_sender1, policy_receiver1), + ]) .expect("Failed to create referenced storage"), ); @@ -178,6 +218,9 @@ async fn main(spawner: Spawner) { info!("Spawining type-c service task"); spawner.must_spawn(service_task(controller_context, controllers, [wrapper])); + info!("Spawining power policy task"); + spawner.must_spawn(power_policy_service_task()); + spawner.must_spawn(pd_controller_task(wrapper)); // Sync our internal state with the hardware diff --git a/examples/rt685s-evk/src/bin/type_c_cfu.rs b/examples/rt685s-evk/src/bin/type_c_cfu.rs index 87cfed3a4..91f68d3e9 100644 --- a/examples/rt685s-evk/src/bin/type_c_cfu.rs +++ b/examples/rt685s-evk/src/bin/type_c_cfu.rs @@ -8,6 +8,7 @@ use embassy_imxrt::gpio::{Input, Inverter, Pull}; use embassy_imxrt::i2c::Async; use embassy_imxrt::i2c::master::{Config, I2cMaster}; use embassy_imxrt::{bind_interrupts, peripherals}; +use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender}; use embassy_sync::mutex::Mutex; use embassy_sync::pubsub::PubSubChannel; use embassy_time::Timer; @@ -16,18 +17,20 @@ use embedded_cfu_protocol::protocol_definitions::*; use embedded_cfu_protocol::protocol_definitions::{FwUpdateOffer, FwUpdateOfferResponse, FwVersion}; use embedded_services::cfu::component::InternalResponseData; use embedded_services::cfu::component::RequestData; -use embedded_services::power::policy::{CommsMessage, DeviceId as PowerId}; +use embedded_services::power::policy::{CommsMessage, DeviceId as PowerId, policy}; use embedded_services::type_c::ControllerId; use embedded_services::type_c::controller::Context; use embedded_services::{GlobalRawMutex, IntrusiveList, cfu}; use embedded_services::{error, info}; use embedded_usb_pd::GlobalPortId; +use power_policy_service::PowerPolicy; use static_cell::StaticCell; use tps6699x::asynchronous::embassy as tps6699x; use type_c_service::driver::tps6699x::{self as tps6699x_drv}; use type_c_service::service::Service; use type_c_service::wrapper::ControllerWrapper; -use type_c_service::wrapper::backing::{ReferencedStorage, Storage}; +use type_c_service::wrapper::backing::{IntermediateStorage, ReferencedStorage, Storage}; +use type_c_service::wrapper::proxy::PowerProxyDevice; extern crate rt685s_evk_example; @@ -47,7 +50,14 @@ impl type_c_service::wrapper::FwOfferValidator for Validator { type BusMaster<'a> = I2cMaster<'a, Async>; type BusDevice<'a> = I2cDevice<'a, GlobalRawMutex, BusMaster<'a>>; type Tps6699xMutex<'a> = Mutex>>; -type Wrapper<'a> = ControllerWrapper<'a, GlobalRawMutex, Tps6699xMutex<'a>, Validator>; +type Wrapper<'a> = ControllerWrapper< + 'a, + GlobalRawMutex, + Tps6699xMutex<'a>, + DynamicSender<'a, policy::RequestData>, + DynamicReceiver<'a, policy::RequestData>, + Validator, +>; type Controller<'a> = tps6699x::controller::Controller>; type Interrupt<'a> = tps6699x::Interrupt<'a, GlobalRawMutex, BusDevice<'a>>; @@ -155,7 +165,15 @@ async fn fw_update_task() { #[embassy_executor::task] async fn power_policy_service_task() { - power_policy_service::task::task(Default::default()) + static POWER_POLICY: static_cell::StaticCell< + PowerPolicy>, DynamicReceiver<'static, policy::RequestData>>, + > = static_cell::StaticCell::new(); + let power_policy = + POWER_POLICY.init(PowerPolicy::create(Default::default()).expect("Failed to create power policy")); + + // TODO: remove once power policy task accepts context + Timer::after_millis(100).await; + power_policy_service::task::task(power_policy) .await .expect("Failed to start power policy service task"); } @@ -243,13 +261,40 @@ async fn main(spawner: Spawner) { controller_context, CONTROLLER0_ID, CONTROLLER0_CFU_ID, - [(PORT0_ID, PORT0_PWR_ID), (PORT1_ID, PORT1_PWR_ID)], + [PORT0_ID, PORT1_ID], )); - static REFERENCED: StaticCell> = StaticCell::new(); - let referenced = REFERENCED.init( + static INTERMEDIATE: StaticCell> = StaticCell::new(); + let intermediate = INTERMEDIATE.init( storage - .create_referenced() + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL0: StaticCell> = StaticCell::new(); + let policy_channel0 = POLICY_CHANNEL0.init(Channel::new()); + let policy_sender0 = policy_channel0.dyn_sender(); + let policy_receiver0 = policy_channel0.dyn_receiver(); + + static POLICY_CHANNEL1: StaticCell> = StaticCell::new(); + let policy_channel1 = POLICY_CHANNEL1.init(Channel::new()); + let policy_sender1 = policy_channel1.dyn_sender(); + let policy_receiver1 = policy_channel1.dyn_receiver(); + + static REFERENCED: StaticCell< + ReferencedStorage< + TPS66994_NUM_PORTS, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced = REFERENCED.init( + intermediate + .try_create_referenced([ + (PORT0_PWR_ID, policy_sender0, policy_receiver0), + (PORT1_PWR_ID, policy_sender1, policy_receiver1), + ]) .expect("Failed to create referenced storage"), ); diff --git a/examples/std/Cargo.lock b/examples/std/Cargo.lock index d714101dc..ec7c39e2d 100644 --- a/examples/std/Cargo.lock +++ b/examples/std/Cargo.lock @@ -23,6 +23,56 @@ dependencies = [ "memchr", ] +[[package]] +name = "anstream" +version = "0.6.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43d5b281e737544384e969a5ccad3f1cdd24b48086a0fc1b2a5262a26b8f4f4a" +dependencies = [ + "anstyle", + "anstyle-parse", + "anstyle-query", + "anstyle-wincon", + "colorchoice", + "is_terminal_polyfill", + "utf8parse", +] + +[[package]] +name = "anstyle" +version = "1.0.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5192cca8006f1fd4f7237516f40fa183bb07f8fbdfedaa0036de5ea9b0b45e78" + +[[package]] +name = "anstyle-parse" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e7644824f0aa2c7b9384579234ef10eb7efb6a0deb83f9630a49594dd9c15c2" +dependencies = [ + "utf8parse", +] + +[[package]] +name = "anstyle-query" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "40c48f72fd53cd289104fc64099abca73db4166ad86ea0b4341abe65af83dadc" +dependencies = [ + "windows-sys 0.60.2", +] + +[[package]] +name = "anstyle-wincon" +version = "3.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "291e6a250ff86cd4a820112fb8898808a366d8f9f58ce16d1f538353ad55747d" +dependencies = [ + "anstyle", + "once_cell_polyfill", + "windows-sys 0.60.2", +] + [[package]] name = "anyhow" version = "1.0.99" @@ -86,17 +136,6 @@ dependencies = [ "winnow 0.7.13", ] -[[package]] -name = "atty" -version = "0.2.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8" -dependencies = [ - "hermit-abi", - "libc", - "winapi", -] - [[package]] name = "autocfg" version = "1.5.0" @@ -286,6 +325,12 @@ dependencies = [ "log", ] +[[package]] +name = "colorchoice" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b05b61dc5112cbb17e4b6cd61790d9845d13888356391624cbe7e41efeac1e75" + [[package]] name = "const-init" version = "1.0.0" @@ -747,6 +792,7 @@ dependencies = [ "cortex-m-rt", "critical-section", "document-features", + "embassy-futures", "embassy-sync", "embassy-time", "embedded-batteries-async", @@ -794,16 +840,26 @@ dependencies = [ ] [[package]] -name = "env_logger" -version = "0.9.3" +name = "env_filter" +version = "0.1.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a12e6657c4c97ebab115a42dcee77225f7f482cdd841cf7088c657a42e9e00e7" +checksum = "1bf3c259d255ca70051b30e2e95b5446cdb8949ac4cd22c0d7fd634d89f568e2" dependencies = [ - "atty", - "humantime", "log", "regex", - "termcolor", +] + +[[package]] +name = "env_logger" +version = "0.11.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "13c863f0904021b108aa8b2f55046443e6b1ebde8fd4a15c399893aae4fa069f" +dependencies = [ + "anstream", + "anstyle", + "env_filter", + "jiff", + "log", ] [[package]] @@ -906,21 +962,6 @@ version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95505c38b4572b2d910cecb0281560f54b440a19336cbbcb27bf6ce6adc6f5a8" -[[package]] -name = "hermit-abi" -version = "0.1.19" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33" -dependencies = [ - "libc", -] - -[[package]] -name = "humantime" -version = "2.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b112acc8b3adf4b107a8ec20977da0273a8c386765a3ec0229bd500a1443f9f" - [[package]] name = "ident_case" version = "1.0.1" @@ -946,6 +987,12 @@ dependencies = [ "quote", ] +[[package]] +name = "is_terminal_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695" + [[package]] name = "itertools" version = "0.10.5" @@ -970,6 +1017,30 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4a5f13b858c8d314ee3e8f639011f7ccefe71f97f96e50151fb991f267928e2c" +[[package]] +name = "jiff" +version = "0.2.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e67e8da4c49d6d9909fe03361f9b620f58898859f5c7aded68351e85e71ecf50" +dependencies = [ + "jiff-static", + "log", + "portable-atomic", + "portable-atomic-util", + "serde_core", +] + +[[package]] +name = "jiff-static" +version = "0.2.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e0c84ee7f197eca9a86c6fd6cb771e55eb991632f15f2bc3ca6ec838929e6e78" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.106", +] + [[package]] name = "kdl" version = "6.3.4" @@ -1252,6 +1323,12 @@ version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" +[[package]] +name = "once_cell_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe" + [[package]] name = "paste" version = "1.0.15" @@ -1296,6 +1373,15 @@ version = "1.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f84267b20a16ea918e43c6a88433c2d54fa145c92a811b5b047ccbe153674483" +[[package]] +name = "portable-atomic-util" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d8a2f0d8d040d7848a709caf78912debcc3f33ee4b3cac47d73d1e1069e83507" +dependencies = [ + "portable-atomic", +] + [[package]] name = "power-policy-service" version = "0.1.0" @@ -1444,18 +1530,28 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" [[package]] name = "serde" -version = "1.0.219" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" dependencies = [ "serde_derive", ] [[package]] name = "serde_derive" -version = "1.0.219" +version = "1.0.228" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" dependencies = [ "proc-macro2", "quote", @@ -1604,15 +1700,6 @@ version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "55937e1799185b12863d447f42597ed69d9928686b8d88a1df17376a097d8369" -[[package]] -name = "termcolor" -version = "1.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" -dependencies = [ - "winapi-util", -] - [[package]] name = "thermal-service" version = "0.1.0" @@ -1820,6 +1907,12 @@ version = "0.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6d49784317cd0d1ee7ec5c716dd598ec5b4483ea832a2dced265471cc0f690ae" +[[package]] +name = "utf8parse" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821" + [[package]] name = "uuid" version = "1.17.0" @@ -1865,37 +1958,6 @@ dependencies = [ "vcell", ] -[[package]] -name = "winapi" -version = "0.3.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" -dependencies = [ - "winapi-i686-pc-windows-gnu", - "winapi-x86_64-pc-windows-gnu", -] - -[[package]] -name = "winapi-i686-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" - -[[package]] -name = "winapi-util" -version = "0.1.10" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0978bf7171b3d90bac376700cb56d606feb40f251a475a5d6634613564460b22" -dependencies = [ - "windows-sys 0.60.2", -] - -[[package]] -name = "winapi-x86_64-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" - [[package]] name = "windows" version = "0.61.3" diff --git a/examples/std/Cargo.toml b/examples/std/Cargo.toml index 39ffeb396..20d53f7aa 100644 --- a/examples/std/Cargo.toml +++ b/examples/std/Cargo.toml @@ -41,7 +41,7 @@ embedded-fans-async = "0.2.0" thermal-service = { path = "../../thermal-service", features = ["log"] } thermal-service-messages = { path = "../../thermal-service-messages" } -env_logger = "0.9.0" +env_logger = "0.11.8" log = "0.4.14" heapless = "0.8.0" static_cell = "2" diff --git a/examples/std/src/bin/power_policy.rs b/examples/std/src/bin/power_policy.rs index af2ee9608..7608e214a 100644 --- a/examples/std/src/bin/power_policy.rs +++ b/examples/std/src/bin/power_policy.rs @@ -1,11 +1,20 @@ use embassy_executor::{Executor, Spawner}; -use embassy_sync::{blocking_mutex::raw::NoopRawMutex, once_lock::OnceLock, pubsub::PubSubChannel}; +use embassy_sync::{ + blocking_mutex::raw::NoopRawMutex, + channel::{self, Channel}, + mutex::Mutex, + pubsub::PubSubChannel, +}; use embassy_time::{self as _, Timer}; use embedded_services::{ + GlobalRawMutex, broadcaster::immediate as broadcaster, - power::policy::{self, ConsumerPowerCapability, PowerCapability, device, flags}, + power::policy::{ + self, ConsumerPowerCapability, Error, PowerCapability, ProviderPowerCapability, device::DeviceTrait, flags, + }, }; use log::*; +use power_policy_service::PowerPolicy; use static_cell::StaticCell; const LOW_POWER: PowerCapability = PowerCapability { @@ -18,195 +27,221 @@ const HIGH_POWER: PowerCapability = PowerCapability { current_ma: 3000, }; -struct ExampleDevice { - device: policy::device::Device, +const DEVICE0_ID: policy::DeviceId = policy::DeviceId(0); +const DEVICE1_ID: policy::DeviceId = policy::DeviceId(1); + +const PER_CALL_DELAY_MS: u64 = 1000; + +struct ExampleDevice<'a> { + sender: channel::DynamicSender<'a, policy::policy::RequestData>, } -impl ExampleDevice { - fn new(id: policy::DeviceId) -> Self { - Self { - device: policy::device::Device::new(id), - } +impl<'a> ExampleDevice<'a> { + fn new(sender: channel::DynamicSender<'a, policy::policy::RequestData>) -> Self { + Self { sender } } - async fn process_request(&self) -> Result<(), policy::Error> { - let request = self.device.receive().await; - match request.command { - device::CommandData::ConnectAsConsumer(capability) => { - info!( - "Device {} received connect consumer at {:#?}", - self.device.id().0, - capability - ); - } - device::CommandData::ConnectAsProvider(capability) => { - info!( - "Device {} received connect provider at {:#?}", - self.device.id().0, - capability - ); - } - device::CommandData::Disconnect => { - info!("Device {} received disconnect", self.device.id().0); - } - } + pub async fn simulate_attach(&mut self) { + self.sender.send(policy::policy::RequestData::Attached).await; + } - request.respond(Ok(policy::device::ResponseData::Complete)); - Ok(()) + pub async fn simulate_update_consumer_power_capability(&mut self, capability: Option) { + self.sender + .send(policy::policy::RequestData::UpdatedConsumerCapability(capability)) + .await; } -} -impl policy::device::DeviceContainer for ExampleDevice { - fn get_power_policy_device(&self) -> &policy::device::Device { - &self.device + pub async fn simulate_detach(&mut self) { + self.sender.send(policy::policy::RequestData::Detached).await; } -} -#[embassy_executor::task] -async fn device_task0(device: &'static ExampleDevice) { - loop { - if let Err(e) = device.process_request().await { - error!("Error processing request: {e:?}"); - } + pub async fn simulate_update_requested_provider_power_capability( + &mut self, + capability: Option, + ) { + self.sender + .send(policy::policy::RequestData::RequestedProviderCapability(capability)) + .await } } -#[embassy_executor::task] -async fn device_task1(device: &'static ExampleDevice) { - loop { - if let Err(e) = device.process_request().await { - error!("Error processing request: {e:?}"); - } +impl DeviceTrait for ExampleDevice<'_> { + async fn disconnect(&mut self) -> Result<(), Error> { + debug!("ExampleDevice disconnect"); + Ok(()) + } + + async fn connect_provider(&mut self, capability: ProviderPowerCapability) -> Result<(), Error> { + debug!("ExampleDevice connect_provider with {capability:?}"); + Ok(()) + } + + async fn connect_consumer(&mut self, capability: ConsumerPowerCapability) -> Result<(), Error> { + debug!("ExampleDevice connect_consumer with {capability:?}"); + Ok(()) } } #[embassy_executor::task] -async fn run(spawner: Spawner) { +async fn run(_spawner: Spawner) { embedded_services::init().await; info!("Creating device 0"); - static DEVICE0: OnceLock = OnceLock::new(); - let device0_mock = DEVICE0.get_or_init(|| ExampleDevice::new(policy::DeviceId(0))); - policy::register_device(device0_mock).unwrap(); - spawner.must_spawn(device_task0(device0_mock)); - let device0 = device0_mock.device.try_device_action().await.unwrap(); + static DEVICE0_EVENT_CHANNEL: StaticCell> = StaticCell::new(); + let device0_event_channel = DEVICE0_EVENT_CHANNEL.init(Channel::new()); + static DEVICE0: StaticCell> = StaticCell::new(); + let device0 = DEVICE0.init(Mutex::new(ExampleDevice::new(device0_event_channel.dyn_sender()))); + static DEVICE0_REGISTRATION: StaticCell< + policy::device::Device< + 'static, + Mutex, + channel::DynamicReceiver<'static, policy::policy::RequestData>, + >, + > = StaticCell::new(); + let device0_registration = DEVICE0_REGISTRATION.init(policy::device::Device::new( + DEVICE0_ID, + device0, + device0_event_channel.dyn_receiver(), + )); + policy::register_device(device0_registration).unwrap(); info!("Creating device 1"); - static DEVICE1: OnceLock = OnceLock::new(); - let device1_mock = DEVICE1.get_or_init(|| ExampleDevice::new(policy::DeviceId(1))); - policy::register_device(device1_mock).unwrap(); - spawner.must_spawn(device_task1(device1_mock)); - let device1 = device1_mock.device.try_device_action().await.unwrap(); + static DEVICE1_EVENT_CHANNEL: StaticCell> = StaticCell::new(); + let device1_event_channel = DEVICE1_EVENT_CHANNEL.init(Channel::new()); + static DEVICE1: StaticCell> = StaticCell::new(); + let device1 = DEVICE1.init(Mutex::new(ExampleDevice::new(device1_event_channel.dyn_sender()))); + static DEVICE1_REGISTRATION: StaticCell< + policy::device::Device< + 'static, + Mutex, + channel::DynamicReceiver<'static, policy::policy::RequestData>, + >, + > = StaticCell::new(); + let device1_registration = DEVICE1_REGISTRATION.init(policy::device::Device::new( + DEVICE1_ID, + device1, + device1_event_channel.dyn_receiver(), + )); + policy::register_device(device1_registration).unwrap(); // Plug in device 0, should become current consumer info!("Connecting device 0"); - let device0 = device0.attach().await.unwrap(); - device0 - .notify_consumer_power_capability(Some(ConsumerPowerCapability { + { + let mut dev0 = device0.lock().await; + dev0.simulate_attach().await; + dev0.simulate_update_consumer_power_capability(Some(ConsumerPowerCapability { capability: LOW_POWER, flags: flags::Consumer::none().with_unconstrained_power(), })) - .await - .unwrap(); + .await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; // Plug in device 1, should become current consumer info!("Connecting device 1"); - let device1 = device1.attach().await.unwrap(); - device1 - .notify_consumer_power_capability(Some(HIGH_POWER.into())) - .await - .unwrap(); + { + let mut dev1 = device1.lock().await; + dev1.simulate_attach().await; + dev1.simulate_update_consumer_power_capability(Some(HIGH_POWER.into())) + .await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; // Unplug device 0, device 1 should remain current consumer - info!("Unpluging device 0"); - let device0 = device0.detach().await.unwrap(); + info!("Unplugging device 0"); + { + let mut dev0 = device0.lock().await; + dev0.simulate_detach().await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; // Plug in device 0, device 1 should remain current consumer info!("Connecting device 0"); - let device0 = device0.attach().await.unwrap(); - device0 - .notify_consumer_power_capability(Some(LOW_POWER.into())) - .await - .unwrap(); + { + let mut dev0 = device0.lock().await; + dev0.simulate_attach().await; + dev0.simulate_update_consumer_power_capability(Some(LOW_POWER.into())) + .await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; // Unplug device 1, device 0 should become current consumer info!("Unplugging device 1"); - let device1 = device1.detach().await.unwrap(); + { + let mut dev1 = device1.lock().await; + dev1.simulate_detach().await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; // Replug device 1, device 1 becomes current consumer info!("Connecting device 1"); - let device1 = device1.attach().await.unwrap(); - device1 - .notify_consumer_power_capability(Some(HIGH_POWER.into())) - .await - .unwrap(); + { + let mut dev1 = device1.lock().await; + dev1.simulate_attach().await; + dev1.simulate_update_consumer_power_capability(Some(HIGH_POWER.into())) + .await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; - // Disconnect consumer device 0, device 1 should remain current consumer + // Detach consumer device 0, device 1 should remain current consumer // Device 0 should not be able to consume after device 1 is unplugged - info!("Disconnecting device 0"); - device0.notify_consumer_power_capability(None).await.unwrap(); - let device1 = device1.detach().await.unwrap(); - - // Switch to provider on device0 - info!("Device 0 requesting provider"); - device0 - .request_provider_power_capability(LOW_POWER.into()) - .await - .unwrap(); - Timer::after_millis(250).await; - info!( - "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await - ); + info!("Detach device 0"); + { + let mut dev0 = device0.lock().await; + dev0.simulate_update_consumer_power_capability(None).await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; + { + let mut dev1 = device1.lock().await; + dev1.simulate_detach().await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; + + // Switch device 0 to provider + info!("Device 0 switch to provider"); + { + let mut dev0 = device0.lock().await; + dev0.simulate_update_requested_provider_power_capability(Some(HIGH_POWER.into())) + .await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; + + // Attach device 1 and request provider info!("Device 1 attach and requesting provider"); - let device1 = device1.attach().await.unwrap(); - device1 - .request_provider_power_capability(LOW_POWER.into()) - .await - .unwrap(); - // Wait for the provider to be connected - Timer::after_millis(250).await; - info!( - "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await - ); - - // Provider upgrade should fail because device 0 is already connected + { + let mut dev1 = device1.lock().await; + dev1.simulate_attach().await; + dev1.simulate_update_requested_provider_power_capability(Some(LOW_POWER.into())) + .await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; + + // Provider upgrade should fail because device 0 is already connected at high power info!("Device 1 attempting provider upgrade"); - device1 - .request_provider_power_capability(HIGH_POWER.into()) - .await - .unwrap(); - // Wait for the upgrade flow to complete - Timer::after_millis(250).await; - info!( - "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await - ); + { + let mut dev1 = device1.lock().await; + dev1.simulate_update_requested_provider_power_capability(Some(HIGH_POWER.into())) + .await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; // Disconnect device 0 info!("Device 0 disconnecting"); - device0.detach().await.unwrap(); - // Wait for the detach flow to complete - Timer::after_millis(250).await; - info!( - "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await - ); + { + let mut dev0 = device0.lock().await; + dev0.simulate_detach().await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; // Provider upgrade should succeed now info!("Device 1 attempting provider upgrade"); - device1 - .request_provider_power_capability(HIGH_POWER.into()) - .await - .unwrap(); - // Wait for the upgrade flow to complete - Timer::after_millis(250).await; - info!( - "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await - ); + { + let mut dev1 = device1.lock().await; + dev1.simulate_update_requested_provider_power_capability(Some(HIGH_POWER.into())) + .await; + } + Timer::after_millis(PER_CALL_DELAY_MS).await; } #[embassy_executor::task] @@ -235,10 +270,17 @@ async fn receiver_task() { } #[embassy_executor::task] -async fn power_policy_task(config: power_policy_service::config::Config) { - power_policy_service::task::task(config) - .await - .expect("Failed to start power policy service task"); +async fn power_policy_task() { + static POWER_POLICY: StaticCell< + PowerPolicy< + Mutex>, + channel::DynamicReceiver<'static, policy::policy::RequestData>, + >, + > = StaticCell::new(); + let power_policy = POWER_POLICY.init(PowerPolicy::create(Default::default()).unwrap()); + loop { + power_policy.process().await.unwrap(); + } } fn main() { @@ -247,7 +289,7 @@ fn main() { static EXECUTOR: StaticCell = StaticCell::new(); let executor = EXECUTOR.init(Executor::new()); executor.run(|spawner| { - spawner.must_spawn(power_policy_task(power_policy_service::config::Config::default())); + spawner.must_spawn(power_policy_task()); spawner.must_spawn(run(spawner)); spawner.must_spawn(receiver_task()); }); diff --git a/examples/std/src/bin/type_c/basic.rs b/examples/std/src/bin/type_c/basic.rs index 1f8441fe5..14ca5ff1d 100644 --- a/examples/std/src/bin/type_c/basic.rs +++ b/examples/std/src/bin/type_c/basic.rs @@ -1,8 +1,8 @@ use embassy_executor::{Executor, Spawner}; use embassy_sync::once_lock::OnceLock; use embassy_time::Timer; +use embedded_services::IntrusiveList; use embedded_services::type_c::{Cached, ControllerId, controller}; -use embedded_services::{IntrusiveList, power}; use embedded_usb_pd::ucsi::lpm; use embedded_usb_pd::{GlobalPortId, PdError as Error}; use log::*; @@ -11,7 +11,6 @@ use static_cell::StaticCell; const CONTROLLER0_ID: ControllerId = ControllerId(0); const PORT0_ID: GlobalPortId = GlobalPortId(0); const PORT1_ID: GlobalPortId = GlobalPortId(1); -const POWER0_ID: power::policy::DeviceId = power::policy::DeviceId(0); mod test_controller { use embedded_services::type_c::controller::{ControllerStatus, PortStatus}; @@ -21,7 +20,6 @@ mod test_controller { pub struct Controller<'a> { pub controller: controller::Device<'a>, - pub power_policy: power::policy::device::Device, } impl controller::DeviceContainer for Controller<'_> { @@ -30,17 +28,10 @@ mod test_controller { } } - impl power::policy::device::DeviceContainer for Controller<'_> { - fn get_power_policy_device(&self) -> &power::policy::device::Device { - &self.power_policy - } - } - impl<'a> Controller<'a> { - pub fn new(id: ControllerId, power_id: power::policy::DeviceId, ports: &'a [GlobalPortId]) -> Self { + pub fn new(id: ControllerId, ports: &'a [GlobalPortId]) -> Self { Self { controller: controller::Device::new(id, ports), - power_policy: power::policy::device::Device::new(power_id), } } @@ -129,7 +120,7 @@ async fn controller_task(controller_list: &'static IntrusiveList) { static PORTS: [GlobalPortId; 2] = [PORT0_ID, PORT1_ID]; - let controller = CONTROLLER.get_or_init(|| test_controller::Controller::new(CONTROLLER0_ID, POWER0_ID, &PORTS)); + let controller = CONTROLLER.get_or_init(|| test_controller::Controller::new(CONTROLLER0_ID, &PORTS)); controller::register_controller(controller_list, controller).unwrap(); loop { diff --git a/examples/std/src/bin/type_c/external.rs b/examples/std/src/bin/type_c/external.rs index 36a27b588..0a3ae0e0d 100644 --- a/examples/std/src/bin/type_c/external.rs +++ b/examples/std/src/bin/type_c/external.rs @@ -1,8 +1,10 @@ //! Low-level example of external messaging with a simple type-C service use embassy_executor::{Executor, Spawner}; +use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender}; use embassy_sync::mutex::Mutex; use embassy_sync::pubsub::PubSubChannel; use embassy_time::Timer; +use embedded_services::power::policy::policy; use embedded_services::{ GlobalRawMutex, IntrusiveList, power, type_c::{Cached, ControllerId, controller::Context}, @@ -133,13 +135,34 @@ fn create_wrapper(controller_context: &'static Context) -> &'static mut Wrapper< controller_context, CONTROLLER0_ID, 0, // CFU component ID (unused) - [(PORT0_ID, POWER0_ID)], + [PORT0_ID], )); - static REFERENCED: StaticCell> = + + static INTERMEDIATE: StaticCell> = StaticCell::new(); - let referenced = REFERENCED.init( + let intermediate = INTERMEDIATE.init( backing_storage - .create_referenced() + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL: StaticCell> = StaticCell::new(); + let policy_channel = POLICY_CHANNEL.init(Channel::new()); + + let policy_sender = policy_channel.dyn_sender(); + let policy_receiver = policy_channel.dyn_receiver(); + + static REFERENCED: StaticCell< + type_c_service::wrapper::backing::ReferencedStorage< + 1, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced = REFERENCED.init( + intermediate + .try_create_referenced([(POWER0_ID, policy_sender, policy_receiver)]) .expect("Failed to create referenced storage"), ); diff --git a/examples/std/src/bin/type_c/service.rs b/examples/std/src/bin/type_c/service.rs index 5b7d3373a..6823ae095 100644 --- a/examples/std/src/bin/type_c/service.rs +++ b/examples/std/src/bin/type_c/service.rs @@ -1,8 +1,10 @@ -use embassy_executor::{Executor, Spawner}; +use embassy_executor::Executor; +use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender}; use embassy_sync::mutex::Mutex; use embassy_sync::once_lock::OnceLock; use embassy_sync::pubsub::PubSubChannel; use embassy_time::Timer; +use embedded_services::power::policy::policy; use embedded_services::power::{self}; use embedded_services::type_c::ControllerId; use embedded_services::type_c::controller::Context; @@ -11,13 +13,15 @@ use embedded_usb_pd::GlobalPortId; use embedded_usb_pd::ado::Ado; use embedded_usb_pd::type_c::Current; use log::*; +use power_policy_service::PowerPolicy; use static_cell::StaticCell; use std_examples::type_c::mock_controller; use std_examples::type_c::mock_controller::Wrapper; use type_c_service::service::Service; use type_c_service::service::config::Config; -use type_c_service::wrapper::backing::{ReferencedStorage, Storage}; +use type_c_service::wrapper::backing::Storage; use type_c_service::wrapper::message::*; +use type_c_service::wrapper::proxy::PowerProxyDevice; const NUM_PD_CONTROLLERS: usize = 1; const CONTROLLER0_ID: ControllerId = ControllerId(0); @@ -89,12 +93,7 @@ async fn controller_task( } #[embassy_executor::task] -async fn task( - spawner: Spawner, - wrapper: &'static Wrapper<'static>, - controller: &'static Mutex>, - state: &'static mock_controller::ControllerState, -) { +async fn task(state: &'static mock_controller::ControllerState) { embedded_services::init().await; // Register debug accessory listener @@ -102,8 +101,6 @@ async fn task( let listener = LISTENER.get_or_init(debug::Listener::new); comms::register_endpoint(listener, &listener.tp).await.unwrap(); - info!("Starting controller task"); - spawner.must_spawn(controller_task(wrapper, controller)); // Wait for controller to be registered Timer::after_secs(1).await; @@ -130,7 +127,15 @@ async fn task( #[embassy_executor::task] async fn power_policy_service_task() { - power_policy_service::task::task(Default::default()) + static POWER_POLICY: static_cell::StaticCell< + PowerPolicy>, DynamicReceiver<'static, policy::RequestData>>, + > = static_cell::StaticCell::new(); + let power_policy = + POWER_POLICY.init(PowerPolicy::create(Default::default()).expect("Failed to create power policy")); + + // TODO: remove once power policy task accepts context + Timer::after_millis(100).await; + power_policy_service::task::task(power_policy) .await .expect("Failed to start power policy service task"); } @@ -169,7 +174,7 @@ async fn service_task( fn create_wrapper( context: &'static Context, ) -> ( - &'static mut Wrapper<'static>, + &'static Wrapper<'static>, &'static Mutex>, &'static mock_controller::ControllerState, ) { @@ -181,12 +186,34 @@ fn create_wrapper( context, CONTROLLER0_ID, 0, // CFU component ID (unused) - [(PORT0_ID, POWER0_ID)], + [PORT0_ID], )); - static REFERENCED: StaticCell> = StaticCell::new(); - let referenced = REFERENCED.init( + + static INTERMEDIATE: StaticCell> = + StaticCell::new(); + let intermediate = INTERMEDIATE.init( storage - .create_referenced() + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL: StaticCell> = StaticCell::new(); + let policy_channel = POLICY_CHANNEL.init(Channel::new()); + + let policy_sender = policy_channel.dyn_sender(); + let policy_receiver = policy_channel.dyn_receiver(); + + static REFERENCED: StaticCell< + type_c_service::wrapper::backing::ReferencedStorage< + 1, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced = REFERENCED.init( + intermediate + .try_create_referenced([(POWER0_ID, policy_sender, policy_receiver)]) .expect("Failed to create referenced storage"), ); @@ -225,6 +252,7 @@ fn main() { executor.run(|spawner| { spawner.must_spawn(power_policy_service_task()); spawner.must_spawn(service_task(controller_context, controller_list, [wrapper])); - spawner.must_spawn(task(spawner, wrapper, controller, state)); + spawner.must_spawn(task(state)); + spawner.must_spawn(controller_task(wrapper, controller)); }); } diff --git a/examples/std/src/bin/type_c/ucsi.rs b/examples/std/src/bin/type_c/ucsi.rs index bd8a617b5..e13cc83dc 100644 --- a/examples/std/src/bin/type_c/ucsi.rs +++ b/examples/std/src/bin/type_c/ucsi.rs @@ -1,10 +1,13 @@ use crate::mock_controller::Wrapper; use embassy_executor::{Executor, Spawner}; +use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender}; use embassy_sync::mutex::Mutex; use embassy_sync::pubsub::PubSubChannel; +use embassy_time::Timer; use embedded_services::GlobalRawMutex; use embedded_services::IntrusiveList; -use embedded_services::power::policy::{self, PowerCapability}; +use embedded_services::power::policy::PowerCapability; +use embedded_services::power::policy::policy; use embedded_services::type_c::ControllerId; use embedded_services::type_c::controller::Context; use embedded_services::type_c::external::UcsiResponseResult; @@ -15,19 +18,21 @@ use embedded_usb_pd::ucsi::ppm::get_capability::ResponseData as UcsiCapabilities use embedded_usb_pd::ucsi::ppm::set_notification_enable::NotificationEnable; use embedded_usb_pd::ucsi::{Command, lpm, ppm}; use log::*; +use power_policy_service::PowerPolicy; use static_cell::StaticCell; use std_examples::type_c::mock_controller; use type_c_service::service::Service; use type_c_service::service::config::Config; -use type_c_service::wrapper::backing::{ReferencedStorage, Storage}; +use type_c_service::wrapper::backing::Storage; +use type_c_service::wrapper::proxy::PowerProxyDevice; const NUM_PD_CONTROLLERS: usize = 2; const CONTROLLER0_ID: ControllerId = ControllerId(0); const CONTROLLER1_ID: ControllerId = ControllerId(1); const PORT0_ID: GlobalPortId = GlobalPortId(0); -const POWER0_ID: policy::DeviceId = policy::DeviceId(0); +const POWER0_ID: embedded_services::power::policy::DeviceId = embedded_services::power::policy::DeviceId(0); const PORT1_ID: GlobalPortId = GlobalPortId(1); -const POWER1_ID: policy::DeviceId = policy::DeviceId(1); +const POWER1_ID: embedded_services::power::policy::DeviceId = embedded_services::power::policy::DeviceId(1); const CFU0_ID: u8 = 0x00; const CFU1_ID: u8 = 0x01; @@ -172,7 +177,15 @@ async fn wrapper_task(wrapper: &'static mock_controller::Wrapper<'static>) { #[embassy_executor::task] async fn power_policy_service_task() { - power_policy_service::task::task(Default::default()) + static POWER_POLICY: static_cell::StaticCell< + PowerPolicy>, DynamicReceiver<'static, policy::RequestData>>, + > = static_cell::StaticCell::new(); + let power_policy = + POWER_POLICY.init(PowerPolicy::create(Default::default()).expect("Failed to create power policy")); + + // TODO: remove once power policy task accepts context + Timer::after_millis(100).await; + power_policy_service::task::task(power_policy) .await .expect("Failed to start power policy service task"); } @@ -223,11 +236,32 @@ async fn type_c_service_task(spawner: Spawner) { let context = CONTEXT.init(embedded_services::type_c::controller::Context::new()); static STORAGE0: StaticCell> = StaticCell::new(); - let storage0 = STORAGE0.init(Storage::new(context, CONTROLLER0_ID, CFU0_ID, [(PORT0_ID, POWER0_ID)])); - static REFERENCED0: StaticCell> = StaticCell::new(); - let referenced0 = REFERENCED0.init( + let storage0 = STORAGE0.init(Storage::new(context, CONTROLLER0_ID, CFU0_ID, [PORT0_ID])); + + static INTERMEDIATE0: StaticCell> = + StaticCell::new(); + let intermediate0 = INTERMEDIATE0.init( storage0 - .create_referenced() + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL0: StaticCell> = StaticCell::new(); + let policy_channel0 = POLICY_CHANNEL0.init(Channel::new()); + let policy_sender0 = policy_channel0.dyn_sender(); + let policy_receiver0 = policy_channel0.dyn_receiver(); + + static REFERENCED0: StaticCell< + type_c_service::wrapper::backing::ReferencedStorage< + 1, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced0 = REFERENCED0.init( + intermediate0 + .try_create_referenced([(POWER0_ID, policy_sender0, policy_receiver0)]) .expect("Failed to create referenced storage"), ); @@ -242,11 +276,31 @@ async fn type_c_service_task(spawner: Spawner) { ); static STORAGE1: StaticCell> = StaticCell::new(); - let storage1 = STORAGE1.init(Storage::new(context, CONTROLLER1_ID, CFU1_ID, [(PORT1_ID, POWER1_ID)])); - static REFERENCED1: StaticCell> = StaticCell::new(); - let referenced1 = REFERENCED1.init( + let storage1 = STORAGE1.init(Storage::new(context, CONTROLLER1_ID, CFU1_ID, [PORT1_ID])); + static INTERMEDIATE1: StaticCell> = + StaticCell::new(); + let intermediate1 = INTERMEDIATE1.init( storage1 - .create_referenced() + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL1: StaticCell> = StaticCell::new(); + let policy_channel1 = POLICY_CHANNEL1.init(Channel::new()); + let policy_sender1 = policy_channel1.dyn_sender(); + let policy_receiver1 = policy_channel1.dyn_receiver(); + + static REFERENCED1: StaticCell< + type_c_service::wrapper::backing::ReferencedStorage< + 1, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced1 = REFERENCED1.init( + intermediate1 + .try_create_referenced([(POWER1_ID, policy_sender1, policy_receiver1)]) .expect("Failed to create referenced storage"), ); diff --git a/examples/std/src/bin/type_c/unconstrained.rs b/examples/std/src/bin/type_c/unconstrained.rs index c9f489b88..e148edbd4 100644 --- a/examples/std/src/bin/type_c/unconstrained.rs +++ b/examples/std/src/bin/type_c/unconstrained.rs @@ -1,22 +1,26 @@ use crate::mock_controller::Wrapper; use embassy_executor::Executor; +use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender}; use embassy_sync::mutex::Mutex; use embassy_sync::pubsub::PubSubChannel; use embassy_time::Timer; use embedded_services::power::policy::PowerCapability; +use embedded_services::power::policy::policy; use embedded_services::power::{self}; use embedded_services::type_c::ControllerId; use embedded_services::type_c::controller::Context; use embedded_services::{GlobalRawMutex, IntrusiveList}; use embedded_usb_pd::GlobalPortId; use log::*; +use power_policy_service::PowerPolicy; use static_cell::StaticCell; use std_examples::type_c::mock_controller; use type_c_service::service::Service; use type_c_service::service::config::Config; -use type_c_service::wrapper::backing::{ReferencedStorage, Storage}; const NUM_PD_CONTROLLERS: usize = 3; +use type_c_service::wrapper::backing::{IntermediateStorage, ReferencedStorage, Storage}; +use type_c_service::wrapper::proxy::PowerProxyDevice; const CONTROLLER0_ID: ControllerId = ControllerId(0); const PORT0_ID: GlobalPortId = GlobalPortId(0); @@ -99,7 +103,15 @@ async fn task(state: [&'static mock_controller::ControllerState; NUM_PD_CONTROLL #[embassy_executor::task] async fn power_policy_service_task() { - power_policy_service::task::task(Default::default()) + static POWER_POLICY: static_cell::StaticCell< + PowerPolicy>, DynamicReceiver<'static, policy::RequestData>>, + > = static_cell::StaticCell::new(); + let power_policy = + POWER_POLICY.init(PowerPolicy::create(Default::default()).expect("Failed to create power policy")); + + // TODO: remove once power policy task accepts context + Timer::after_millis(100).await; + power_policy_service::task::task(power_policy) .await .expect("Failed to start power policy service task"); } @@ -139,20 +151,36 @@ async fn service_task( fn main() { env_logger::builder().filter_level(log::LevelFilter::Trace).init(); - static EXECUTOR: StaticCell = StaticCell::new(); - let executor = EXECUTOR.init(Executor::new()); - static CONTEXT: StaticCell = StaticCell::new(); let context = CONTEXT.init(embedded_services::type_c::controller::Context::new()); static CONTROLLER_LIST: StaticCell = StaticCell::new(); let controller_list = CONTROLLER_LIST.init(IntrusiveList::new()); - static STORAGE: StaticCell> = StaticCell::new(); - let storage = STORAGE.init(Storage::new(context, CONTROLLER0_ID, CFU0_ID, [(PORT0_ID, POWER0_ID)])); - static REFERENCED: StaticCell> = StaticCell::new(); - let referenced = REFERENCED.init( - storage - .create_referenced() + static STORAGE0: StaticCell> = StaticCell::new(); + let storage0 = STORAGE0.init(Storage::new(context, CONTROLLER0_ID, CFU0_ID, [PORT0_ID])); + static INTERMEDIATE0: StaticCell> = StaticCell::new(); + let intermediate0 = INTERMEDIATE0.init( + storage0 + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL0: StaticCell> = StaticCell::new(); + let policy_channel0 = POLICY_CHANNEL0.init(Channel::new()); + let policy_sender0 = policy_channel0.dyn_sender(); + let policy_receiver0 = policy_channel0.dyn_receiver(); + + static REFERENCED0: StaticCell< + ReferencedStorage< + 1, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced0 = REFERENCED0.init( + intermediate0 + .try_create_referenced([(POWER0_ID, policy_sender0, policy_receiver0)]) .expect("Failed to create referenced storage"), ); @@ -165,18 +193,37 @@ fn main() { mock_controller::Wrapper::try_new( controller0, Default::default(), - referenced, + referenced0, crate::mock_controller::Validator, ) .expect("Failed to create wrapper"), ); static STORAGE1: StaticCell> = StaticCell::new(); - let storage1 = STORAGE1.init(Storage::new(context, CONTROLLER1_ID, CFU1_ID, [(PORT1_ID, POWER1_ID)])); - static REFERENCED1: StaticCell> = StaticCell::new(); - let referenced1 = REFERENCED1.init( + let storage1 = STORAGE1.init(Storage::new(context, CONTROLLER1_ID, CFU1_ID, [PORT1_ID])); + static INTERMEDIATE1: StaticCell> = StaticCell::new(); + let intermediate1 = INTERMEDIATE1.init( storage1 - .create_referenced() + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL1: StaticCell> = StaticCell::new(); + let policy_channel1 = POLICY_CHANNEL1.init(Channel::new()); + let policy_sender1 = policy_channel1.dyn_sender(); + let policy_receiver1 = policy_channel1.dyn_receiver(); + + static REFERENCED1: StaticCell< + ReferencedStorage< + 1, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced1 = REFERENCED1.init( + intermediate1 + .try_create_referenced([(POWER1_ID, policy_sender1, policy_receiver1)]) .expect("Failed to create referenced storage"), ); @@ -196,11 +243,30 @@ fn main() { ); static STORAGE2: StaticCell> = StaticCell::new(); - let storage2 = STORAGE2.init(Storage::new(context, CONTROLLER2_ID, CFU2_ID, [(PORT2_ID, POWER2_ID)])); - static REFERENCED2: StaticCell> = StaticCell::new(); - let referenced2 = REFERENCED2.init( + let storage2 = STORAGE2.init(Storage::new(context, CONTROLLER2_ID, CFU2_ID, [PORT2_ID])); + static INTERMEDIATE2: StaticCell> = StaticCell::new(); + let intermediate2 = INTERMEDIATE2.init( storage2 - .create_referenced() + .try_create_intermediate() + .expect("Failed to create intermediate storage"), + ); + + static POLICY_CHANNEL2: StaticCell> = StaticCell::new(); + let policy_channel2 = POLICY_CHANNEL2.init(Channel::new()); + let policy_sender2 = policy_channel2.dyn_sender(); + let policy_receiver2 = policy_channel2.dyn_receiver(); + + static REFERENCED2: StaticCell< + ReferencedStorage< + 1, + GlobalRawMutex, + DynamicSender<'_, policy::RequestData>, + DynamicReceiver<'_, policy::RequestData>, + >, + > = StaticCell::new(); + let referenced2 = REFERENCED2.init( + intermediate2 + .try_create_referenced([(POWER2_ID, policy_sender2, policy_receiver2)]) .expect("Failed to create referenced storage"), ); @@ -219,6 +285,8 @@ fn main() { .expect("Failed to create wrapper"), ); + static EXECUTOR: StaticCell = StaticCell::new(); + let executor = EXECUTOR.init(Executor::new()); executor.run(|spawner| { spawner.must_spawn(power_policy_service_task()); spawner.must_spawn(service_task(context, controller_list, [wrapper0, wrapper1, wrapper2])); diff --git a/examples/std/src/lib/type_c/mock_controller.rs b/examples/std/src/lib/type_c/mock_controller.rs index 766373508..a3cda41be 100644 --- a/examples/std/src/lib/type_c/mock_controller.rs +++ b/examples/std/src/lib/type_c/mock_controller.rs @@ -1,8 +1,8 @@ -use embassy_sync::{mutex::Mutex, signal::Signal}; +use embassy_sync::{channel, mutex::Mutex, signal::Signal}; use embedded_cfu_protocol::protocol_definitions::{FwUpdateOfferResponse, HostToken}; use embedded_services::{ GlobalRawMutex, - power::policy::PowerCapability, + power::policy::{PowerCapability, policy}, type_c::{ controller::{ AttnVdm, ControllerStatus, DpConfig, DpPinConfig, DpStatus, OtherVdm, PdStateMachineConfig, PortStatus, @@ -16,7 +16,6 @@ use embedded_usb_pd::{LocalPortId, PdError}; use embedded_usb_pd::{PowerRole, type_c::Current}; use embedded_usb_pd::{type_c::ConnectionState, ucsi::lpm}; use log::{debug, info, trace}; -use std::cell::Cell; pub struct ControllerState { events: Signal, @@ -41,22 +40,24 @@ impl ControllerState { } else { ConnectionState::Attached }); + + let mut events = PortEvent::none(); match role { PowerRole::Source => { status.available_source_contract = Some(capability); status.unconstrained_power = unconstrained; + events.status.set_new_power_contract_as_provider(true); } PowerRole::Sink => { status.available_sink_contract = Some(capability); status.unconstrained_power = unconstrained; + events.status.set_new_power_contract_as_consumer(true); + events.status.set_sink_ready(true); } } *self.status.lock().await = status; - let mut events = PortEvent::none(); events.status.set_plug_inserted_or_removed(true); - events.status.set_new_power_contract_as_consumer(true); - events.status.set_sink_ready(true); self.events.signal(events); } @@ -97,19 +98,19 @@ impl Default for ControllerState { pub struct Controller<'a> { state: &'a ControllerState, - events: Cell, + events: PortEvent, } impl<'a> Controller<'a> { pub fn new(state: &'a ControllerState) -> Self { Self { state, - events: Cell::new(PortEvent::none()), + events: PortEvent::none(), } } /// Function to demonstrate calling functions directly on the controller - pub fn custom_function(&self) { + pub fn custom_function(&mut self) { info!("Custom function called on controller"); } } @@ -120,14 +121,14 @@ impl embedded_services::type_c::controller::Controller for Controller<'_> { async fn wait_port_event(&mut self) -> Result<(), Error> { let events = self.state.events.wait().await; trace!("Port event: {events:#?}"); - self.events.set(events); + self.events = self.events.union(events); Ok(()) } async fn clear_port_events(&mut self, _port: LocalPortId) -> Result> { - let events = self.events.get(); + let events = self.events; debug!("Clear port events: {events:#?}"); - self.events.set(PortEvent::none()); + self.events = PortEvent::none(); Ok(events) } @@ -336,5 +337,11 @@ impl type_c_service::wrapper::FwOfferValidator for Validator { } } -pub type Wrapper<'a> = - type_c_service::wrapper::ControllerWrapper<'a, GlobalRawMutex, Mutex>, Validator>; +pub type Wrapper<'a> = type_c_service::wrapper::ControllerWrapper< + 'a, + GlobalRawMutex, + Mutex>, + channel::DynamicSender<'a, policy::RequestData>, + channel::DynamicReceiver<'a, policy::RequestData>, + Validator, +>; diff --git a/power-policy-service/Cargo.toml b/power-policy-service/Cargo.toml index 8e661bb0d..9be53aed5 100644 --- a/power-policy-service/Cargo.toml +++ b/power-policy-service/Cargo.toml @@ -19,6 +19,14 @@ embedded-services.workspace = true log = { workspace = true, optional = true } heapless.workspace = true +[dev-dependencies] +static_cell.workspace = true +critical-section = { workspace = true, features = ["std"] } +embassy-time = { workspace = true, features = ["std", "generic-queue-8"] } +tokio = { workspace = true, features = ["rt", "macros", "time"] } +env_logger = "0.11.8" +log = { workspace = true } + [features] default = [] defmt = [ diff --git a/power-policy-service/src/consumer.rs b/power-policy-service/src/consumer.rs index 0600193fc..b7bd0a398 100644 --- a/power-policy-service/src/consumer.rs +++ b/power-policy-service/src/consumer.rs @@ -31,14 +31,17 @@ fn cmp_consumer_capability( (a.capability, a_is_current).cmp(&(b.capability, b_is_current)) } -impl PowerPolicy { +impl + 'static> PowerPolicy +where + D::Inner: DeviceTrait, +{ /// Iterate over all devices to determine what is best power port provides the highest power async fn find_best_consumer(&self, state: &InternalState) -> Result, Error> { let mut best_consumer = None; let current_consumer_id = state.current_consumer_state.map(|f| f.device_id); for node in self.context.devices() { - let device = node.data::().ok_or(Error::InvalidDevice)?; + let device = node.data::>().ok_or(Error::InvalidDevice)?; let consumer_capability = device.consumer_capability().await; // Don't consider consumers below minimum threshold @@ -92,7 +95,7 @@ impl PowerPolicy { // Count how many available unconstrained devices we have let mut unconstrained_new = UnconstrainedState::default(); for node in self.context.devices() { - let device = node.data::().ok_or(Error::InvalidDevice)?; + let device = node.data::>().ok_or(Error::InvalidDevice)?; if let Some(capability) = device.consumer_capability().await { if capability.flags.unconstrained_power() { unconstrained_new.available += 1; @@ -197,18 +200,19 @@ impl PowerPolicy { } state.current_consumer_state = None; - // Disconnect the current consumer if needed - if let Ok(consumer) = self - .context - .try_policy_action::(current_consumer.device_id) - .await - { - info!( - "Device {}, disconnecting current consumer", - current_consumer.device_id.0 - ); + let consumer_device = self.context.get_device(current_consumer.device_id)?; + if matches!(consumer_device.state.lock().await.state(), State::ConnectedConsumer(_)) { + // Disconnect the current consumer if needed + info!("Device{}: Disconnecting current consumer", current_consumer.device_id.0); // disconnect current consumer and set idle - consumer.disconnect().await?; + consumer_device.device.lock().await.disconnect().await?; + if let Err(e) = consumer_device.state.lock().await.disconnect(false) { + // This should never happen because we check the state above, log an error instead of a panic + error!( + "Device{}: Disconnect transition failed: {:#?}", + current_consumer.device_id.0, e + ); + } } // If no chargers are registered, they won't receive the new power capability. @@ -226,28 +230,30 @@ impl PowerPolicy { } info!("Device {}, connecting new consumer", new_consumer.device_id.0); - if let Ok(idle) = self - .context - .try_policy_action::(new_consumer.device_id) - .await - { - idle.connect_consumer(new_consumer.consumer_power_capability).await?; - self.post_consumer_connected(state, new_consumer).await?; - } else if let Ok(provider) = self - .context - .try_policy_action::(new_consumer.device_id) + let device = self.context.get_device(new_consumer.device_id)?; + let device_state = device.state.lock().await.state(); + + if let e @ Err(_) = device + .state + .lock() .await + .connect_consumer(new_consumer.consumer_power_capability) { - provider + error!( + "Device{}: Not ready to connect consumer, state: {:#?}", + device.id().0, + device_state + ); + e + } else { + device + .device + .lock() + .await .connect_consumer(new_consumer.consumer_power_capability) .await?; - state.current_consumer_state = Some(new_consumer); - self.post_consumer_connected(state, new_consumer).await?; - } else { - error!("Error obtaining device in idle state"); + self.post_consumer_connected(state, new_consumer).await } - - Ok(()) } /// Determines and connects the best external power diff --git a/power-policy-service/src/lib.rs b/power-policy-service/src/lib.rs index c43719f81..1f49cd899 100644 --- a/power-policy-service/src/lib.rs +++ b/power-policy-service/src/lib.rs @@ -2,8 +2,11 @@ use core::ops::DerefMut; use embassy_sync::mutex::Mutex; use embedded_services::GlobalRawMutex; -use embedded_services::power::policy::device::Device; -use embedded_services::power::policy::{action, policy, *}; +use embedded_services::event::Receiver; +use embedded_services::power::policy::device::{Device, DeviceTrait, State}; +use embedded_services::power::policy::policy::RequestData; +use embedded_services::power::policy::{policy, *}; +use embedded_services::sync::Lockable; use embedded_services::{comms, error, info}; pub mod config; @@ -29,9 +32,12 @@ struct InternalState { } /// Power policy state -pub struct PowerPolicy { +pub struct PowerPolicy> +where + D::Inner: DeviceTrait, +{ /// Power policy context - context: policy::ContextToken, + context: policy::ContextToken, /// State state: Mutex, /// Comms endpoint @@ -40,7 +46,10 @@ pub struct PowerPolicy { config: config::Config, } -impl PowerPolicy { +impl + 'static> PowerPolicy +where + D::Inner: DeviceTrait, +{ /// Create a new power policy pub fn create(config: config::Config) -> Option { Some(Self { @@ -51,38 +60,75 @@ impl PowerPolicy { }) } - async fn process_notify_attach(&self) -> Result<(), Error> { - self.context.send_response(Ok(policy::ResponseData::Complete)).await; - Ok(()) + async fn process_notify_attach(&self, device: &Device<'_, D, R>) { + if let Err(e) = device.state.lock().await.attach() { + error!("Device{}: Invalid state for attach: {:#?}", device.id().0, e); + } } - async fn process_notify_detach(&self, device: &device::Device) -> Result<(), Error> { - self.context.send_response(Ok(policy::ResponseData::Complete)).await; - self.remove_connected_provider(device.id()).await; - self.update_current_consumer().await?; - Ok(()) + async fn process_notify_detach(&self, device: &Device<'_, D, R>) -> Result<(), Error> { + device.state.lock().await.detach(); + self.update_current_consumer().await } - async fn process_notify_consumer_power_capability(&self) -> Result<(), Error> { - self.context.send_response(Ok(policy::ResponseData::Complete)).await; - self.update_current_consumer().await?; - Ok(()) + async fn process_notify_consumer_power_capability( + &self, + device: &Device<'_, D, R>, + capability: Option, + ) -> Result<(), Error> { + if let Err(e) = device.state.lock().await.update_consumer_power_capability(capability) { + error!( + "Device{}: Invalid state for notify consumer capability, catching up: {:#?}", + device.id().0, + e, + ); + } + + self.update_current_consumer().await } - async fn process_request_provider_power_capabilities(&self, device: DeviceId) -> Result<(), Error> { - self.context.send_response(Ok(policy::ResponseData::Complete)).await; - self.connect_provider(device).await; - Ok(()) + async fn process_request_provider_power_capabilities( + &self, + device: &Device<'_, D, R>, + capability: Option, + ) -> Result<(), Error> { + if let Err(e) = device + .state + .lock() + .await + .update_requested_provider_power_capability(capability) + { + error!( + "Device{}: Invalid state for notify consumer capability, catching up: {:#?}", + device.id().0, + e, + ); + } + + self.connect_provider(device.id()).await } - async fn process_notify_disconnect(&self, device: &device::Device) -> Result<(), Error> { - self.context.send_response(Ok(policy::ResponseData::Complete)).await; - if let Some(consumer) = self.state.lock().await.current_consumer_state.take() { - info!("Device{}: Connected consumer disconnected", consumer.device_id.0); + async fn process_notify_disconnect(&self, device: &Device<'_, D, R>) -> Result<(), Error> { + if let Err(e) = device.state.lock().await.disconnect(true) { + error!( + "Device{}: Invalid state for notify disconnect, catching up: {:#?}", + device.id().0, + e, + ); + } + + if self + .state + .lock() + .await + .current_consumer_state + .is_some_and(|current| current.device_id == device.id()) + { + info!("Device{}: Connected consumer disconnected", device.id().0); self.disconnect_chargers().await?; self.comms_notify(CommsMessage { - data: CommsData::ConsumerDisconnected(consumer.device_id), + data: CommsData::ConsumerDisconnected(device.id()), }) .await; } @@ -124,31 +170,33 @@ impl PowerPolicy { let device = self.context.get_device(request.id)?; match request.data { - policy::RequestData::NotifyAttached => { + policy::RequestData::Attached => { info!("Received notify attached from device {}", device.id().0); - self.process_notify_attach().await + self.process_notify_attach(device).await; + Ok(()) } - policy::RequestData::NotifyDetached => { + policy::RequestData::Detached => { info!("Received notify detached from device {}", device.id().0); self.process_notify_detach(device).await } - policy::RequestData::NotifyConsumerCapability(capability) => { + policy::RequestData::UpdatedConsumerCapability(capability) => { info!( "Device{}: Received notify consumer capability: {:#?}", device.id().0, capability, ); - self.process_notify_consumer_power_capability().await + self.process_notify_consumer_power_capability(device, capability).await } - policy::RequestData::RequestProviderCapability(capability) => { + policy::RequestData::RequestedProviderCapability(capability) => { info!( "Device{}: Received request provider capability: {:#?}", device.id().0, capability, ); - self.process_request_provider_power_capabilities(device.id()).await + self.process_request_provider_power_capabilities(device, capability) + .await } - policy::RequestData::NotifyDisconnect => { + policy::RequestData::Disconnected => { info!("Received notify disconnect from device {}", device.id().0); self.process_notify_disconnect(device).await } @@ -162,4 +210,7 @@ impl PowerPolicy { } } -impl comms::MailboxDelegate for PowerPolicy {} +impl + 'static> comms::MailboxDelegate for PowerPolicy where + D::Inner: DeviceTrait +{ +} diff --git a/power-policy-service/src/provider.rs b/power-policy-service/src/provider.rs index 830509576..04d2b0287 100644 --- a/power-policy-service/src/provider.rs +++ b/power-policy-service/src/provider.rs @@ -3,7 +3,7 @@ //! the system is in unlimited power state. In this mode up to [provider_unlimited](super::Config::provider_unlimited) //! is provided to each device. Above this threshold, the system is in limited power state. //! In this mode [provider_limited](super::Config::provider_limited) is provided to each device -use embedded_services::{debug, trace}; +use embedded_services::{debug, event::Receiver, power::policy::policy::RequestData, trace}; use super::*; @@ -25,30 +25,27 @@ pub(super) struct State { state: PowerState, } -impl PowerPolicy { +impl + 'static> PowerPolicy +where + D::Inner: DeviceTrait, +{ /// Attempt to connect the requester as a provider - pub(super) async fn connect_provider(&self, requester_id: DeviceId) { + pub(super) async fn connect_provider(&self, requester_id: DeviceId) -> Result<(), Error> { trace!("Device{}: Attempting to connect as provider", requester_id.0); - let requester = match self.context.get_device(requester_id) { - Ok(device) => device, - Err(_) => { - error!("Device{}: Invalid device", requester_id.0); - return; - } - }; + let requester = self.context.get_device(requester_id)?; let requested_power_capability = match requester.requested_provider_capability().await { Some(cap) => cap, // Requester is no longer requesting power _ => { - info!("Device{}: No-longer requesting power", requester.id().0); - return; + error!("Device{}: No-longer requesting power", requester.id().0); + return Err(Error::CannotProvide(None)); } }; - let mut state = self.state.lock().await; + let mut policy_state = self.state.lock().await; let mut total_power_mw = 0; // Determine total requested power draw - for device in self.context.devices().iter_only::() { + for device in self.context.devices().iter_only::>() { let target_provider_cap = if device.id() == requester_id { // Use the requester's requested power capability // this handles both new connections and upgrade requests @@ -58,17 +55,17 @@ impl PowerPolicy { device.provider_capability().await }; total_power_mw += target_provider_cap.map_or(0, |cap| cap.capability.max_power_mw()); + } - if total_power_mw > self.config.limited_power_threshold_mw { - state.current_provider_state.state = PowerState::Limited; - } else { - state.current_provider_state.state = PowerState::Unlimited; - } + if total_power_mw > self.config.limited_power_threshold_mw { + policy_state.current_provider_state.state = PowerState::Limited; + } else { + policy_state.current_provider_state.state = PowerState::Unlimited; } - debug!("New power state: {:?}", state.current_provider_state.state); + debug!("New power state: {:?}", policy_state.current_provider_state.state); - let target_power = match state.current_provider_state.state { + let target_power = match policy_state.current_provider_state.state { PowerState::Limited => ProviderPowerCapability { capability: self.config.provider_limited, flags: requested_power_capability.flags, @@ -87,36 +84,20 @@ impl PowerPolicy { } }; - let connected = if let Ok(action) = self.context.try_policy_action::(requester.id()).await { - if let Err(e) = action.connect_provider(target_power).await { - error!("Device{}: Failed to connect as provider, {:#?}", requester.id().0, e); - } else { - self.post_provider_connected(&mut state, requester.id(), target_power) - .await; - } - Ok(()) - } else if let Ok(action) = self - .context - .try_policy_action::(requester.id()) - .await - { - if let Err(e) = action.connect_provider(target_power).await { - error!("Device{}: Failed to connect as provider, {:#?}", requester.id().0, e); - } else { - self.post_provider_connected(&mut state, requester.id(), target_power) - .await; - } - Ok(()) + let device = self.context.get_device(requester_id)?; + if let e @ Err(_) = device.state.lock().await.connect_provider(target_power) { + let state = device.state.lock().await.state(); + error!( + "Device{}: Cannot provide, device is in state {:#?}", + device.id().0, + state + ); + e } else { - Err(Error::InvalidState( - device::StateKind::Idle, - requester.state().await.kind(), - )) - }; - - // Don't need to do anything special, the device is responsible for attempting to reconnect - if let Err(e) = connected { - error!("Device{}: Failed to connect as provider, {:#?}", requester.id().0, e); + device.device.lock().await.connect_provider(target_power).await?; + self.post_provider_connected(&mut policy_state, requester_id, target_power) + .await; + Ok(()) } } diff --git a/power-policy-service/src/task.rs b/power-policy-service/src/task.rs index 916b2909f..aa8a9a83a 100644 --- a/power-policy-service/src/task.rs +++ b/power-policy-service/src/task.rs @@ -1,7 +1,12 @@ -use embassy_sync::once_lock::OnceLock; -use embedded_services::{comms, error, info}; +use embedded_services::{ + comms, error, + event::Receiver, + info, + power::policy::{device::DeviceTrait, policy::RequestData}, + sync::Lockable, +}; -use crate::{PowerPolicy, config}; +use crate::PowerPolicy; #[derive(Debug, Clone, Copy)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] @@ -12,15 +17,13 @@ pub enum InitError { RegistrationFailed, } -pub async fn task(config: config::Config) -> Result { +pub async fn task + 'static>( + policy: &'static PowerPolicy, +) -> Result +where + D::Inner: DeviceTrait, +{ info!("Starting power policy task"); - static POLICY: OnceLock = OnceLock::new(); - let policy = if let Some(policy) = PowerPolicy::create(config) { - POLICY.get_or_init(|| policy) - } else { - error!("Power policy service already initialized"); - return Err(InitError::AlreadyInitialized); - }; if comms::register_endpoint(policy, &policy.tp).await.is_err() { error!("Failed to register power policy endpoint"); diff --git a/power-policy-service/tests/common/mock.rs b/power-policy-service/tests/common/mock.rs new file mode 100644 index 000000000..443842958 --- /dev/null +++ b/power-policy-service/tests/common/mock.rs @@ -0,0 +1,82 @@ +#![allow(clippy::unwrap_used)] +use embassy_sync::signal::Signal; +use embedded_services::power::policy::device::{DeviceTrait, InternalState}; +use embedded_services::power::policy::flags::Consumer; +use embedded_services::power::policy::policy::RequestData; +use embedded_services::power::policy::{ConsumerPowerCapability, Error, PowerCapability, ProviderPowerCapability}; +use embedded_services::{GlobalRawMutex, event, info}; + +#[derive(Debug, Clone, PartialEq, Eq)] +#[allow(dead_code)] +pub enum FnCall { + ConnectConsumer(ConsumerPowerCapability), + ConnectProvider(ProviderPowerCapability), + Disconnect, + Reset, +} + +pub struct Mock<'a, S: event::Sender> { + sender: S, + fn_call: &'a Signal, + // Internal state + pub state: InternalState, +} + +impl<'a, S: event::Sender> Mock<'a, S> { + pub fn new(sender: S, fn_call: &'a Signal) -> Self { + Self { + sender, + fn_call, + state: Default::default(), + } + } + + fn record_fn_call(&mut self, fn_call: FnCall) { + let num_fn_calls = self + .fn_call + .try_take() + .map(|(num_fn_calls, _)| num_fn_calls) + .unwrap_or(1); + self.fn_call.signal((num_fn_calls, fn_call)); + } + + pub async fn simulate_consumer_connection(&mut self, capability: PowerCapability) { + self.state.attach().unwrap(); + + self.sender.send(RequestData::Attached).await; + + let capability = Some(ConsumerPowerCapability { + capability, + flags: Consumer::none(), + }); + self.state.update_consumer_power_capability(capability).unwrap(); + self.sender + .send(RequestData::UpdatedConsumerCapability(capability)) + .await; + } + + pub async fn simulate_detach(&mut self) { + self.state.detach(); + self.sender.send(RequestData::Detached).await; + } +} + +impl<'a, S: event::Sender> DeviceTrait for Mock<'a, S> { + async fn connect_consumer(&mut self, capability: ConsumerPowerCapability) -> Result<(), Error> { + info!("Connect consumer {:#?}", capability); + self.record_fn_call(FnCall::ConnectConsumer(capability)); + Ok(()) + } + + async fn connect_provider(&mut self, capability: ProviderPowerCapability) -> Result<(), Error> { + info!("Connect provider: {:#?}", capability); + self.record_fn_call(FnCall::ConnectProvider(capability)); + Ok(()) + } + + async fn disconnect(&mut self) -> Result<(), Error> { + info!("Disconnect"); + self.record_fn_call(FnCall::Disconnect); + Ok(()) + } +} diff --git a/power-policy-service/tests/common/mod.rs b/power-policy-service/tests/common/mod.rs new file mode 100644 index 000000000..8a3587a0f --- /dev/null +++ b/power-policy-service/tests/common/mod.rs @@ -0,0 +1,128 @@ +#![allow(clippy::unwrap_used)] +use embassy_futures::{ + join::join, + select::{Either, select}, +}; +use embassy_sync::{ + channel::{Channel, DynamicReceiver, DynamicSender}, + mutex::Mutex, + signal::Signal, +}; +use embassy_time::{Duration, with_timeout}; +use embedded_services::{ + GlobalRawMutex, + power::policy::{self, DeviceId, PowerCapability, device, policy::RequestData}, +}; +use power_policy_service::PowerPolicy; + +pub mod mock; + +use mock::Mock; +use static_cell::StaticCell; + +use crate::common::mock::FnCall; + +pub const LOW_POWER: PowerCapability = PowerCapability { + voltage_mv: 5000, + current_ma: 1500, +}; + +#[allow(dead_code)] +pub const HIGH_POWER: PowerCapability = PowerCapability { + voltage_mv: 5000, + current_ma: 3000, +}; + +pub const DEFAULT_TIMEOUT: Duration = Duration::from_secs(15); + +const EVENT_CHANNEL_SIZE: usize = 4; + +async fn power_policy_task( + completion_signal: &'static Signal, + power_policy: &'static PowerPolicy< + Mutex>>, + DynamicReceiver<'static, RequestData>, + >, +) { + while let Either::First(result) = select(power_policy.process(), completion_signal.wait()).await { + result.unwrap(); + } +} + +pub async fn run_test>( + timeout: Duration, + test: impl FnOnce( + &'static Mutex>>, + &'static Signal, + &'static Mutex>>, + &'static Signal, + ) -> F, +) { + env_logger::builder().filter_level(log::LevelFilter::Trace).init(); + embedded_services::init().await; + + static DEVICE0_EVENT_CHANNEL: StaticCell> = + StaticCell::new(); + let device0_event_channel = DEVICE0_EVENT_CHANNEL.init(Channel::new()); + let device0_sender = device0_event_channel.dyn_sender(); + let device0_receiver = device0_event_channel.dyn_receiver(); + + static DEVICE0_SIGNAL: StaticCell> = StaticCell::new(); + let device0_signal = DEVICE0_SIGNAL.init(Signal::new()); + static DEVICE0: StaticCell>>> = StaticCell::new(); + let device0 = DEVICE0.init(Mutex::new(Mock::new(device0_sender, device0_signal))); + + static DEVICE0_REGISTRATION: StaticCell< + device::Device< + 'static, + Mutex>>, + DynamicReceiver<'static, RequestData>, + >, + > = StaticCell::new(); + let device0_registration = DEVICE0_REGISTRATION.init(device::Device::new(DeviceId(0), device0, device0_receiver)); + + policy::register_device(device0_registration).unwrap(); + + static DEVICE1_EVENT_CHANNEL: StaticCell> = + StaticCell::new(); + let device1_event_channel = DEVICE1_EVENT_CHANNEL.init(Channel::new()); + let device1_sender = device1_event_channel.dyn_sender(); + let device1_receiver = device1_event_channel.dyn_receiver(); + + static DEVICE1_SIGNAL: StaticCell> = StaticCell::new(); + let device1_signal = DEVICE1_SIGNAL.init(Signal::new()); + static DEVICE1: StaticCell>>> = StaticCell::new(); + let device1 = DEVICE1.init(Mutex::new(Mock::new(device1_sender, device1_signal))); + + static DEVICE1_REGISTRATION: StaticCell< + device::Device< + 'static, + Mutex>>, + DynamicReceiver<'static, RequestData>, + >, + > = StaticCell::new(); + let device1_registration = DEVICE1_REGISTRATION.init(device::Device::new(DeviceId(1), device1, device1_receiver)); + + policy::register_device(device1_registration).unwrap(); + + static POWER_POLICY: StaticCell< + PowerPolicy< + Mutex>>, + DynamicReceiver<'static, RequestData>, + >, + > = StaticCell::new(); + let power_policy = POWER_POLICY.init(power_policy_service::PowerPolicy::create(Default::default()).unwrap()); + + static COMPLETION_SIGNAL: StaticCell> = StaticCell::new(); + let completion_signal = COMPLETION_SIGNAL.init(Signal::new()); + + with_timeout( + timeout, + join(power_policy_task(completion_signal, power_policy), async { + test(device0, device0_signal, device1, device1_signal).await; + completion_signal.signal(()); + }), + ) + .await + .unwrap(); +} diff --git a/power-policy-service/tests/consumer.rs b/power-policy-service/tests/consumer.rs new file mode 100644 index 000000000..bf104cbc2 --- /dev/null +++ b/power-policy-service/tests/consumer.rs @@ -0,0 +1,135 @@ +#![allow(clippy::unwrap_used)] +use embassy_sync::{channel::DynamicSender, mutex::Mutex, signal::Signal}; +use embassy_time::{Duration, TimeoutError, with_timeout}; +use embedded_services::{ + GlobalRawMutex, + power::policy::{ConsumerPowerCapability, flags::Consumer, policy::RequestData}, +}; + +mod common; + +use common::LOW_POWER; + +use crate::common::{ + DEFAULT_TIMEOUT, HIGH_POWER, + mock::{FnCall, Mock}, + run_test, +}; + +const PER_CALL_TIMEOUT: Duration = Duration::from_millis(1000); + +/// Test the basic consumer flow with a single device. +async fn test_single( + device0: &'static Mutex>>, + device0_signal: &'static Signal, +) { + // Test initial connection + { + device0.lock().await.simulate_consumer_connection(LOW_POWER).await; + + assert_eq!( + with_timeout(PER_CALL_TIMEOUT, device0_signal.wait()).await.unwrap(), + ( + 1, + FnCall::ConnectConsumer(ConsumerPowerCapability { + capability: LOW_POWER, + flags: Consumer::none(), + }) + ) + ); + device0_signal.reset(); + } + // Test detach + { + device0.lock().await.simulate_detach().await; + + // Power policy shouldn't call any functions on detach so we'll timeout + assert_eq!( + with_timeout(PER_CALL_TIMEOUT, device0_signal.wait()).await, + Err(TimeoutError) + ); + device0_signal.reset(); + } +} + +/// Test swapping to a higher powered device. +async fn test_swap_higher( + device0: &'static Mutex>>, + device0_signal: &'static Signal, + device1: &'static Mutex>>, + device1_signal: &'static Signal, +) { + // Device0 connection at low power + { + device0.lock().await.simulate_consumer_connection(LOW_POWER).await; + + assert_eq!( + with_timeout(PER_CALL_TIMEOUT, device0_signal.wait()).await.unwrap(), + ( + 1, + FnCall::ConnectConsumer(ConsumerPowerCapability { + capability: LOW_POWER, + flags: Consumer::none(), + }) + ) + ); + device0_signal.reset(); + } + // Device1 connection at high power + { + device1.lock().await.simulate_consumer_connection(HIGH_POWER).await; + + assert_eq!( + with_timeout(PER_CALL_TIMEOUT, device0_signal.wait()).await.unwrap(), + (1, FnCall::Disconnect) + ); + device0_signal.reset(); + + assert_eq!( + with_timeout(PER_CALL_TIMEOUT, device1_signal.wait()).await.unwrap(), + ( + 1, + FnCall::ConnectConsumer(ConsumerPowerCapability { + capability: HIGH_POWER, + flags: Consumer::none(), + }) + ) + ); + device1_signal.reset(); + } + // Test detach device1, should reconnect device0 + { + device1.lock().await.simulate_detach().await; + + // Power policy shouldn't call any functions on detach so we'll timeout + assert_eq!( + with_timeout(PER_CALL_TIMEOUT, device1_signal.wait()).await, + Err(TimeoutError) + ); + + assert_eq!( + with_timeout(PER_CALL_TIMEOUT, device0_signal.wait()).await.unwrap(), + ( + 1, + FnCall::ConnectConsumer(ConsumerPowerCapability { + capability: LOW_POWER, + flags: Consumer::none(), + }) + ) + ); + device0_signal.reset(); + } +} + +/// Run all tests, this is temporary to deal with 'static lifetimes until the intrusive list refactor is done. +#[tokio::test] +async fn run_all_tests() { + run_test( + DEFAULT_TIMEOUT, + |device0, device0_signal, device1, device1_signal| async move { + test_single(device0, device0_signal).await; + test_swap_higher(device0, device0_signal, device1, device1_signal).await; + }, + ) + .await; +} diff --git a/type-c-service/src/task.rs b/type-c-service/src/task.rs index 11664a1dc..a66d6d44a 100644 --- a/type-c-service/src/task.rs +++ b/type-c-service/src/task.rs @@ -1,18 +1,20 @@ use core::future::Future; -use embedded_services::{error, info}; +use embedded_services::{error, event, info, power::policy::policy}; use crate::{service::Service, wrapper::ControllerWrapper}; /// Task to run the Type-C service, takes a closure to customize the event loop -pub async fn task_closure<'a, M, C, V, Fut: Future, F: Fn(&'a Service) -> Fut, const N: usize>( +pub async fn task_closure<'a, M, D, S, R, V, Fut: Future, F: Fn(&'a Service) -> Fut, const N: usize>( service: &'static Service<'a>, - wrappers: [&'a ControllerWrapper<'a, M, C, V>; N], + wrappers: [&'a ControllerWrapper<'a, M, D, S, R, V>; N], f: F, ) where M: embassy_sync::blocking_mutex::raw::RawMutex, - C: embedded_services::sync::Lockable, + D: embedded_services::sync::Lockable, + S: event::Sender, + R: event::Receiver, V: crate::wrapper::FwOfferValidator, - ::Inner: embedded_services::type_c::controller::Controller, + ::Inner: embedded_services::type_c::controller::Controller, { info!("Starting type-c task"); @@ -34,14 +36,16 @@ pub async fn task_closure<'a, M, C, V, Fut: Future, F: Fn(&'a Servi } /// Task to run the Type-C service, running the default event loop -pub async fn task<'a, M, C, V, const N: usize>( +pub async fn task<'a, M, D, S, R, V, const N: usize>( service: &'static Service<'a>, - wrappers: [&'a ControllerWrapper<'a, M, C, V>; N], + wrappers: [&'a ControllerWrapper<'a, M, D, S, R, V>; N], ) where M: embassy_sync::blocking_mutex::raw::RawMutex, - C: embedded_services::sync::Lockable, + D: embedded_services::sync::Lockable, + S: event::Sender, + R: event::Receiver, V: crate::wrapper::FwOfferValidator, - ::Inner: embedded_services::type_c::controller::Controller, + ::Inner: embedded_services::type_c::controller::Controller, { task_closure(service, wrappers, |service: &Service| async { if let Err(e) = service.process_next_event().await { diff --git a/type-c-service/src/wrapper/backing.rs b/type-c-service/src/wrapper/backing.rs index 711b5a02e..bd6949573 100644 --- a/type-c-service/src/wrapper/backing.rs +++ b/type-c-service/src/wrapper/backing.rs @@ -21,34 +21,62 @@ //! use embedded_services::type_c::ControllerId; //! use embedded_services::power; //! use embedded_usb_pd::GlobalPortId; -//! use type_c_service::wrapper::backing::{Storage, ReferencedStorage}; -//! -//! -//! const NUM_PORTS: usize = 2; +//! use type_c_service::wrapper::backing::{Storage, IntermediateStorage, ReferencedStorage}; +//! use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender}; +//! use embedded_services::power::policy::policy; //! //! fn init(context: &'static embedded_services::type_c::controller::Context) { -//! static STORAGE: StaticCell> = StaticCell::new(); +//! static STORAGE: StaticCell> = StaticCell::new(); //! let storage = STORAGE.init(Storage::new( //! context, //! ControllerId(0), -//! 0x0, -//! [(GlobalPortId(0), power::policy::DeviceId(0)), (GlobalPortId(1), power::policy::DeviceId(1))], +//! 0x0, // CFU component ID (unused) +//! [GlobalPortId(0)], //! )); -//! static REFERENCED: StaticCell> = StaticCell::new(); -//! let referenced = REFERENCED.init(storage.create_referenced().unwrap()); -//! let _backing = referenced.create_backing().unwrap(); +//! +//! static INTERMEDIATE: StaticCell> = +//! StaticCell::new(); +//! let intermediate = INTERMEDIATE.init(storage.try_create_intermediate().expect("Failed to create intermediate storage")); +//! +//! static POLICY_CHANNEL: StaticCell> = StaticCell::new(); +//! let policy_channel = POLICY_CHANNEL.init(Channel::new()); +//! +//! let policy_sender = policy_channel.dyn_sender(); +//! let policy_receiver = policy_channel.dyn_receiver(); +//! +//! static REFERENCED: StaticCell< +//! type_c_service::wrapper::backing::ReferencedStorage< +//! 1, +//! NoopRawMutex, +//! DynamicSender<'_, policy::RequestData>, +//! DynamicReceiver<'_, policy::RequestData>, +//! >, +//! > = StaticCell::new(); +//! let referenced = REFERENCED.init( +//! intermediate +//! .try_create_referenced([(power::policy::DeviceId(0), policy_sender, policy_receiver)]) +//! .expect("Failed to create referenced storage"), +//! ); //! } //! ``` -use core::cell::{RefCell, RefMut}; +use core::{ + array::from_fn, + cell::{RefCell, RefMut}, +}; use embassy_sync::{ blocking_mutex::raw::RawMutex, + mutex::Mutex, pubsub::{DynImmediatePublisher, DynSubscriber, PubSubChannel}, }; use embassy_time::Instant; use embedded_cfu_protocol::protocol_definitions::ComponentId; use embedded_services::{ - power, + event, + power::{ + self, + policy::{DeviceId, policy}, + }, type_c::{ ControllerId, controller::PortStatus, @@ -57,7 +85,13 @@ use embedded_services::{ }; use embedded_usb_pd::{GlobalPortId, ado::Ado}; -use crate::{PortEventStreamer, wrapper::cfu}; +use crate::{ + PortEventStreamer, + wrapper::{ + cfu, + proxy::{PowerProxyChannel, PowerProxyDevice, PowerProxyReceiver}, + }, +}; /// Per-port state pub struct PortState<'a> { @@ -94,13 +128,14 @@ impl Default for ControllerState { } /// Internal state containing all per-port and per-controller state -struct InternalState<'a, const N: usize> { +struct InternalState<'a, const N: usize, S: event::Sender> { controller_state: ControllerState, port_states: [PortState<'a>; N], + port_power: [PortPower; N], } -impl<'a, const N: usize> InternalState<'a, N> { - fn try_new(storage: &'a Storage) -> Option { +impl<'a, const N: usize, S: event::Sender> InternalState<'a, N, S> { + fn try_new(storage: &'a Storage, power_events: [S; N]) -> Option { let port_states = storage.pd_alerts.each_ref().map(|pd_alert| { Some(PortState { status: PortStatus::new(), @@ -120,11 +155,15 @@ impl<'a, const N: usize> InternalState<'a, N> { // Panic safety: All array elements checked above #[allow(clippy::unwrap_used)] port_states: port_states.map(|s| s.unwrap()), + port_power: power_events.map(|sender| PortPower { + sender, + state: Default::default(), + }), }) } } -impl<'a, const N: usize> DynPortState<'a> for InternalState<'a, N> { +impl<'a, const N: usize, S: event::Sender> DynPortState<'a, S> for InternalState<'a, N, S> { fn num_ports(&self) -> usize { self.port_states.len() } @@ -144,10 +183,18 @@ impl<'a, const N: usize> DynPortState<'a> for InternalState<'a, N> { fn controller_state_mut(&mut self) -> &mut ControllerState { &mut self.controller_state } + + fn port_power(&self) -> &[PortPower] { + &self.port_power + } + + fn port_power_mut(&mut self) -> &mut [PortPower] { + &mut self.port_power + } } /// Trait to erase the generic port count argument -pub trait DynPortState<'a> { +pub trait DynPortState<'a, S: event::Sender> { fn num_ports(&self) -> usize; fn port_states(&self) -> &[PortState<'a>]; @@ -155,17 +202,20 @@ pub trait DynPortState<'a> { fn controller_state(&self) -> &ControllerState; fn controller_state_mut(&mut self) -> &mut ControllerState; + + fn port_power(&self) -> &[PortPower]; + fn port_power_mut(&mut self) -> &mut [PortPower]; } /// Service registration objects -pub struct Registration<'a> { +pub struct Registration<'a, M: RawMutex, R: event::Receiver> { pub context: &'a embedded_services::type_c::controller::Context, pub pd_controller: &'a embedded_services::type_c::controller::Device<'a>, pub cfu_device: &'a embedded_services::cfu::component::CfuDevice, - pub power_devices: &'a [embedded_services::power::policy::device::Device], + pub power_devices: &'a [embedded_services::power::policy::device::Device<'a, Mutex>, R>], } -impl<'a> Registration<'a> { +impl<'a, M: RawMutex, R: event::Receiver> Registration<'a, M, R> { pub fn num_ports(&self) -> usize { self.power_devices.len() } @@ -174,6 +224,11 @@ impl<'a> Registration<'a> { /// PD alerts should be fairly uncommon, four seems like a reasonable number to start with. const MAX_BUFFERED_PD_ALERTS: usize = 4; +pub struct PortPower> { + pub sender: S, + pub state: power::policy::device::InternalState, +} + /// Base storage pub struct Storage<'a, const N: usize, M: RawMutex> { // Registration-related @@ -181,7 +236,7 @@ pub struct Storage<'a, const N: usize, M: RawMutex> { controller_id: ControllerId, pd_ports: [GlobalPortId; N], cfu_device: embedded_services::cfu::component::CfuDevice, - power_devices: [embedded_services::power::policy::device::Device; N], + power_proxy_channels: [PowerProxyChannel; N], // State-related pd_alerts: [PubSubChannel; N], @@ -192,21 +247,61 @@ impl<'a, const N: usize, M: RawMutex> Storage<'a, N, M> { context: &'a embedded_services::type_c::controller::Context, controller_id: ControllerId, cfu_id: ComponentId, - ports: [(GlobalPortId, power::policy::DeviceId); N], + pd_ports: [GlobalPortId; N], ) -> Self { Self { context, controller_id, - pd_ports: ports.map(|(port, _)| port), + pd_ports, cfu_device: embedded_services::cfu::component::CfuDevice::new(cfu_id), - power_devices: ports.map(|(_, device)| embedded_services::power::policy::device::Device::new(device)), + power_proxy_channels: from_fn(|_| PowerProxyChannel::new()), pd_alerts: [const { PubSubChannel::new() }; N], } } - /// Create referenced storage from this storage - pub fn create_referenced(&self) -> Option> { - ReferencedStorage::try_from_storage(self) + /// Create intermediate storage from this storage + pub fn try_create_intermediate(&self) -> Option> { + IntermediateStorage::try_from_storage(self) + } +} + +/// Intermediate storage that holds power proxy devices +pub struct IntermediateStorage<'a, const N: usize, M: RawMutex> { + storage: &'a Storage<'a, N, M>, + power_proxy_devices: [Mutex>; N], + power_proxy_receivers: [Mutex>; N], +} + +impl<'a, const N: usize, M: RawMutex> IntermediateStorage<'a, N, M> { + fn try_from_storage(storage: &'a Storage<'a, N, M>) -> Option { + let mut power_proxy_devices = heapless::Vec::<_, N>::new(); + let mut power_proxy_receivers = heapless::Vec::<_, N>::new(); + + for power_proxy_channel in storage.power_proxy_channels.iter() { + power_proxy_devices + .push(Mutex::new(power_proxy_channel.get_device())) + .ok()?; + power_proxy_receivers + .push(Mutex::new(power_proxy_channel.get_receiver())) + .ok()?; + } + + Some(Self { + storage, + power_proxy_devices: power_proxy_devices.into_array().ok()?, + power_proxy_receivers: power_proxy_receivers.into_array().ok()?, + }) + } + + /// Create referenced storage from this intermediate storage + pub fn try_create_referenced<'b, S: event::Sender, R: event::Receiver>( + &'b self, + policy_args: [(DeviceId, S, R); N], + ) -> Option> + where + 'b: 'a, + { + ReferencedStorage::try_from_intermediate(self, policy_args) } } @@ -214,44 +309,77 @@ impl<'a, const N: usize, M: RawMutex> Storage<'a, N, M> { /// /// To simplify usage, we use interior mutability through a ref cell to avoid having to declare the state /// completely separately. -pub struct ReferencedStorage<'a, const N: usize, M: RawMutex> { - storage: &'a Storage<'a, N, M>, - state: RefCell>, +pub struct ReferencedStorage< + 'a, + const N: usize, + M: RawMutex, + S: event::Sender, + R: event::Receiver, +> { + intermediate: &'a IntermediateStorage<'a, N, M>, + state: RefCell>, pd_controller: embedded_services::type_c::controller::Device<'a>, + power_devices: [embedded_services::power::policy::device::Device<'a, Mutex>, R>; N], } -impl<'a, const N: usize, M: RawMutex> ReferencedStorage<'a, N, M> { - /// Create a new referenced storage from the given storage and controller ID - fn try_from_storage(storage: &'a Storage) -> Option { +impl<'a, const N: usize, M: RawMutex, S: event::Sender, R: event::Receiver> + ReferencedStorage<'a, N, M, S, R> +{ + /// Create a new referenced storage from the given intermediate storage + fn try_from_intermediate( + intermediate: &'a IntermediateStorage<'a, N, M>, + policy_args: [(DeviceId, S, R); N], + ) -> Option { + let mut power_senders = heapless::Vec::<_, N>::new(); + let mut power_devices = heapless::Vec::<_, N>::new(); + + for (i, (device_id, policy_sender, policy_receiver)) in policy_args.into_iter().enumerate() { + power_senders.push(policy_sender).ok()?; + power_devices + .push(embedded_services::power::policy::device::Device::new( + device_id, + intermediate.power_proxy_devices.get(i)?, + policy_receiver, + )) + .ok()?; + } + Some(Self { - storage, - state: RefCell::new(InternalState::try_new(storage)?), + intermediate, + state: RefCell::new(InternalState::try_new( + intermediate.storage, + // Safe because both have N elements + power_senders.into_array().ok()?, + )?), pd_controller: embedded_services::type_c::controller::Device::new( - storage.controller_id, - storage.pd_ports.as_slice(), + intermediate.storage.controller_id, + intermediate.storage.pd_ports.as_slice(), ), + power_devices: power_devices.into_array().ok()?, }) } /// Creates the backing, returns `None` if a backing has already been created - pub fn create_backing<'b>(&'b self) -> Option> + pub fn create_backing<'b>(&'b self) -> Option> where 'b: 'a, { - self.state.try_borrow_mut().ok().map(|state| Backing { + self.state.try_borrow_mut().ok().map(|state| Backing:: { registration: Registration { - context: self.storage.context, + context: self.intermediate.storage.context, pd_controller: &self.pd_controller, - cfu_device: &self.storage.cfu_device, - power_devices: &self.storage.power_devices, + cfu_device: &self.intermediate.storage.cfu_device, + power_devices: &self.power_devices, }, state, + power_receivers: &self.intermediate.power_proxy_receivers, }) } } /// Wrapper around registration and type-erased state -pub struct Backing<'a> { - pub(crate) registration: Registration<'a>, - pub(crate) state: RefMut<'a, dyn DynPortState<'a>>, +pub struct Backing<'a, M: RawMutex, S: event::Sender, R: event::Receiver> { + pub(crate) registration: Registration<'a, M, R>, + pub(crate) state: RefMut<'a, dyn DynPortState<'a, S>>, + pub(crate) power_receivers: &'a [Mutex>], } diff --git a/type-c-service/src/wrapper/cfu.rs b/type-c-service/src/wrapper/cfu.rs index d1b7f7246..581975019 100644 --- a/type-c-service/src/wrapper/cfu.rs +++ b/type-c-service/src/wrapper/cfu.rs @@ -4,6 +4,7 @@ use embassy_futures::select::{Either, select}; use embedded_cfu_protocol::protocol_definitions::*; use embedded_services::cfu::component::{InternalResponseData, RequestData}; use embedded_services::power; +use embedded_services::power::policy::policy; use embedded_services::type_c::controller::Controller; use embedded_services::{debug, error}; @@ -29,9 +30,16 @@ impl FwUpdateState { } } -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl< + 'device, + M: RawMutex, + D: Lockable, + S: event::Sender, + R: event::Receiver, + V: FwOfferValidator, +> ControllerWrapper<'device, M, D, S, R, V> where - ::Inner: Controller, + ::Inner: Controller, { /// Create a new invalid FW version response fn create_invalid_fw_version_response(&self) -> InternalResponseData { @@ -44,7 +52,7 @@ where } /// Process a GetFwVersion command - async fn process_get_fw_version(&self, target: &mut C::Inner) -> InternalResponseData { + async fn process_get_fw_version(&self, target: &mut D::Inner) -> InternalResponseData { let version = match target.get_active_fw_version().await { Ok(v) => v, Err(Error::Pd(e)) => { @@ -75,7 +83,7 @@ where } /// Process a GiveOffer command - async fn process_give_offer(&self, target: &mut C::Inner, offer: &FwUpdateOffer) -> InternalResponseData { + async fn process_give_offer(&self, target: &mut D::Inner, offer: &FwUpdateOffer) -> InternalResponseData { if offer.component_info.component_id != self.registration.cfu_device.component_id() { return Self::create_offer_rejection(); } @@ -97,8 +105,8 @@ where async fn process_abort_update( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, ) -> InternalResponseData { // abort the update process match controller.abort_fw_update().await { @@ -122,8 +130,8 @@ where /// Process a GiveContent command async fn process_give_content( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, content: &FwUpdateContentCommand, ) -> InternalResponseData { let data = if let Some(data) = content.data.get(0..content.header.data_length as usize) { @@ -141,47 +149,14 @@ where // Detach from the power policy so it doesn't attempt to do anything while we are updating let controller_id = self.registration.pd_controller.id(); - let mut detached_all = true; - for power in self.registration.power_devices { + for power in state.port_power_mut() { info!("Controller{}: checking power device", controller_id.0); - if power.state().await != power::policy::device::State::Detached { + if power.state.state() != power::policy::device::State::Detached { info!("Controller{}: Detaching power device", controller_id.0); - if let Err(e) = power.detach().await { - error!("Controller{}: Failed to detach power device: {:?}", controller_id.0, e); - - // Sync to bring the controller to a known state with all services - match self.sync_state_internal(controller, state).await { - Ok(_) => debug!( - "Controller{}: Synced state after detaching power device", - controller_id.0 - ), - Err(Error::Pd(e)) => error!( - "Controller{}: Failed to sync state after detaching power device: {:?}", - controller_id.0, e - ), - Err(Error::Bus(_)) => error!( - "Controller{}: Failed to sync state after detaching power device, bus error", - controller_id.0 - ), - } - - detached_all = false; - break; - } + power.sender.send(policy::RequestData::Detached).await; } } - if !detached_all { - error!( - "Controller{}: Failed to detach all power devices, rejecting offer", - controller_id.0 - ); - return InternalResponseData::ContentResponse(FwUpdateContentResponse::new( - content.header.sequence_num, - CfuUpdateContentResponseStatus::ErrorPrepare, - )); - } - // Need to start the update self.fw_update_ticker.lock().await.reset(); match controller.start_fw_update().await { @@ -258,7 +233,7 @@ where } /// Process a CFU tick - pub async fn process_cfu_tick(&self, controller: &mut C::Inner, state: &mut dyn DynPortState<'_>) { + pub async fn process_cfu_tick(&self, controller: &mut D::Inner, state: &mut dyn DynPortState<'_, S>) { match state.controller_state_mut().fw_update_state { FwUpdateState::Idle => { // No FW update in progress, nothing to do @@ -300,8 +275,8 @@ where /// Process a CFU command pub async fn process_cfu_command( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, command: &RequestData, ) -> InternalResponseData { if state.controller_state().fw_update_state == FwUpdateState::Recovery { diff --git a/type-c-service/src/wrapper/dp.rs b/type-c-service/src/wrapper/dp.rs index 8bae8560f..f25012976 100644 --- a/type-c-service/src/wrapper/dp.rs +++ b/type-c-service/src/wrapper/dp.rs @@ -1,19 +1,26 @@ use super::{ControllerWrapper, FwOfferValidator}; use crate::wrapper::message::OutputDpStatusChanged; use embassy_sync::blocking_mutex::raw::RawMutex; -use embedded_services::{sync::Lockable, trace, type_c::controller::Controller}; +use embedded_services::{event, power::policy::policy, sync::Lockable, trace, type_c::controller::Controller}; use embedded_usb_pd::{Error, LocalPortId}; -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl< + 'device, + M: RawMutex, + D: Lockable, + S: event::Sender, + R: event::Receiver, + V: FwOfferValidator, +> ControllerWrapper<'device, M, D, S, R, V> where - ::Inner: Controller, + ::Inner: Controller, { /// Process a DisplayPort status update by retrieving the current DP status from the `controller` for the appropriate `port`. pub(super) async fn process_dp_status_update( &self, - controller: &mut C::Inner, + controller: &mut D::Inner, port: LocalPortId, - ) -> Result::BusError>> { + ) -> Result::BusError>> { trace!("Processing DP status update event on port {}", port.0); let status = controller.get_dp_status(port).await?; diff --git a/type-c-service/src/wrapper/message.rs b/type-c-service/src/wrapper/message.rs index f8416b3ae..e5650f219 100644 --- a/type-c-service/src/wrapper/message.rs +++ b/type-c-service/src/wrapper/message.rs @@ -31,12 +31,11 @@ pub struct EventPortNotification { } /// Power policy command event data -pub struct EventPowerPolicyCommand<'a> { +pub struct EventPowerPolicyCommand { /// Port ID pub port: LocalPortId, /// Power policy request - pub request: - deferred::Request<'a, GlobalRawMutex, policy::device::CommandData, policy::device::InternalResponseData>, + pub request: policy::device::CommandData, } /// CFU events @@ -58,7 +57,7 @@ pub enum Event<'a> { /// Port notification PortNotification(EventPortNotification), /// Power policy command received - PowerPolicyCommand(EventPowerPolicyCommand<'a>), + PowerPolicyCommand(EventPowerPolicyCommand), /// Command from TCPM ControllerCommand(deferred::Request<'a, GlobalRawMutex, controller::Command, controller::Response<'static>>), /// Cfu event @@ -88,12 +87,9 @@ pub struct OutputPdAlert { } /// Power policy command output data -pub struct OutputPowerPolicyCommand<'a> { +pub struct OutputPowerPolicyCommand { /// Port ID pub port: LocalPortId, - /// Power policy request - pub request: - deferred::Request<'a, GlobalRawMutex, policy::device::CommandData, policy::device::InternalResponseData>, /// Response pub response: policy::device::InternalResponseData, } @@ -158,7 +154,7 @@ pub enum Output<'a> { /// Vendor-defined messaging. Vdm(vdm::Output), /// Power policy command received - PowerPolicyCommand(OutputPowerPolicyCommand<'a>), + PowerPolicyCommand(OutputPowerPolicyCommand), /// TPCM command response ControllerCommand(OutputControllerCommand<'a>), /// CFU recovery tick diff --git a/type-c-service/src/wrapper/mod.rs b/type-c-service/src/wrapper/mod.rs index fb2853c0c..3a45e7634 100644 --- a/type-c-service/src/wrapper/mod.rs +++ b/type-c-service/src/wrapper/mod.rs @@ -27,18 +27,18 @@ use embassy_sync::mutex::Mutex; use embassy_sync::signal::Signal; use embassy_time::Instant; use embedded_cfu_protocol::protocol_definitions::{FwUpdateOffer, FwUpdateOfferResponse, FwVersion}; -use embedded_services::power::policy::device::StateKind; -use embedded_services::power::policy::{self, action}; +use embedded_services::power::policy::policy; use embedded_services::sync::Lockable; use embedded_services::type_c::controller::{self, Controller, PortStatus}; use embedded_services::type_c::event::{PortEvent, PortNotificationSingle, PortPending, PortStatusChanged}; -use embedded_services::{GlobalRawMutex, intrusive_list}; use embedded_services::{debug, error, info, trace, warn}; +use embedded_services::{event, intrusive_list}; use embedded_usb_pd::ado::Ado; use embedded_usb_pd::{Error, LocalPortId, PdError}; -use crate::wrapper::backing::DynPortState; +use crate::wrapper::backing::{DynPortState, PortPower}; use crate::wrapper::message::*; +use crate::wrapper::proxy::PowerProxyReceiver; use crate::{PortEventStreamer, PortEventVariant}; pub mod backing; @@ -48,6 +48,7 @@ mod dp; pub mod message; mod pd; mod power; +pub mod proxy; mod vdm; /// Base interval for checking for FW update timeouts and recovery attempts @@ -68,34 +69,49 @@ pub trait FwOfferValidator { pub const MAX_SUPPORTED_PORTS: usize = 2; /// Common functionality implemented on top of [`embedded_services::type_c::controller::Controller`] -pub struct ControllerWrapper<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> -where - ::Inner: Controller, +pub struct ControllerWrapper< + 'device, + M: RawMutex, + D: Lockable, + S: event::Sender, + R: event::Receiver, + V: FwOfferValidator, +> where + ::Inner: Controller, { - controller: &'device C, + controller: &'device D, /// Trait object for validating firmware versions fw_version_validator: V, /// FW update ticker used to check for timeouts and recovery attempts fw_update_ticker: Mutex, /// Registration information for services - registration: backing::Registration<'device>, + registration: backing::Registration<'device, M, R>, /// State - state: Mutex>>, + state: Mutex>>, /// SW port status event signal sw_status_event: Signal, /// General config config: config::Config, + /// Power proxy receivers + power_proxy_receivers: &'device [Mutex>], } -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl< + 'device, + M: RawMutex, + D: Lockable, + S: event::Sender, + R: event::Receiver, + V: FwOfferValidator, +> ControllerWrapper<'device, M, D, S, R, V> where - ::Inner: Controller, + ::Inner: Controller, { /// Create a new controller wrapper, returns `None` if the backing storage is already in use pub fn try_new( - controller: &'device C, + controller: &'device D, config: config::Config, - storage: &'device backing::ReferencedStorage<'device, N, M>, + storage: &'device backing::ReferencedStorage<'device, N, M, S, R>, fw_version_validator: V, ) -> Option { const { @@ -113,14 +129,10 @@ where registration: backing.registration, state: Mutex::new(backing.state), sw_status_event: Signal::new(), + power_proxy_receivers: backing.power_receivers, }) } - /// Get the power policy devices for this controller. - pub fn power_policy_devices(&self) -> &[policy::device::Device] { - self.registration.power_devices - } - /// Get the cached port status, returns None if the port is invalid pub async fn get_cached_port_status(&self, local_port: LocalPortId) -> Option { self.state @@ -132,7 +144,7 @@ where } /// Synchronize the state between the controller and the internal state - pub async fn sync_state(&self) -> Result<(), Error<::BusError>> { + pub async fn sync_state(&self) -> Result<(), Error<::BusError>> { let mut controller = self.controller.lock().await; let mut state = self.state.lock().await; self.sync_state_internal(&mut controller, state.deref_mut().deref_mut()) @@ -142,9 +154,9 @@ where /// Synchronize the state between the controller and the internal state async fn sync_state_internal( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, - ) -> Result<(), Error<::BusError>> { + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, + ) -> Result<(), Error<::BusError>> { // Sync the controller state with the PD controller for (i, port_state) in state.port_states_mut().iter_mut().enumerate() { let mut status_changed = port_state.sw_status_event; @@ -179,11 +191,11 @@ where /// Handle a plug event async fn process_plug_event( &self, - _controller: &mut C::Inner, - power: &policy::device::Device, + _controller: &mut D::Inner, + power: &mut PortPower, port: LocalPortId, status: &PortStatus, - ) -> Result<(), Error<::BusError>> { + ) -> Result<(), Error<::BusError>> { if port.0 as usize >= self.registration.num_ports() { error!("Invalid port {}", port.0); return PdError::InvalidPort.into(); @@ -192,32 +204,10 @@ where info!("Plug event"); if status.is_connected() { info!("Plug inserted"); - - // Recover if we're not in the correct state - if power.state().await.kind() != StateKind::Detached { - warn!("Power device not in detached state, recovering"); - if let Err(e) = power.detach().await { - error!("Error detaching power device: {:?}", e); - return PdError::Failed.into(); - } - } - - if let Ok(state) = power.try_device_action::().await { - if let Err(e) = state.attach().await { - error!("Error attaching power device: {:?}", e); - return PdError::Failed.into(); - } - } else { - // This should never happen - error!("Power device not in detached state"); - return PdError::InvalidMode.into(); - } + power.sender.send(policy::RequestData::Attached).await; } else { info!("Plug removed"); - if let Err(e) = power.detach().await { - error!("Error detaching power device: {:?}", e); - return PdError::Failed.into(); - }; + power.sender.send(policy::RequestData::Detached).await; } Ok(()) @@ -226,11 +216,11 @@ where /// Process port status changed events async fn process_port_status_changed<'b>( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, local_port_id: LocalPortId, status_event: PortStatusChanged, - ) -> Result, Error<::BusError>> { + ) -> Result, Error<::BusError>> { let global_port_id = self .registration .pd_controller @@ -239,11 +229,12 @@ where let status = controller.get_port_status(local_port_id).await?; trace!("Port{} status: {:#?}", global_port_id.0, status); - - let power = self - .get_power_device(local_port_id) - .ok_or(Error::Pd(PdError::InvalidPort))?; trace!("Port{} status events: {:#?}", global_port_id.0, status_event); + + let power = state + .port_power_mut() + .get_mut(local_port_id.0 as usize) + .ok_or(PdError::InvalidPort)?; if status_event.plug_inserted_or_removed() { self.process_plug_event(controller, power, local_port_id, &status) .await?; @@ -276,11 +267,11 @@ where /// Finalize a port status change output fn finalize_port_status_change( &self, - state: &mut dyn DynPortState<'_>, + state: &mut dyn DynPortState<'_, S>, local_port: LocalPortId, status_event: PortStatusChanged, status: PortStatus, - ) -> Result<(), Error<::BusError>> { + ) -> Result<(), Error<::BusError>> { let port_index = local_port.0 as usize; let global_port_id = self .registration @@ -314,10 +305,10 @@ where /// Finalize a PD alert output fn finalize_pd_alert( &self, - state: &mut dyn DynPortState<'_>, + state: &mut dyn DynPortState<'_, S>, local_port: LocalPortId, alert: Ado, - ) -> Result<(), Error<::BusError>> { + ) -> Result<(), Error<::BusError>> { let port_index = local_port.0 as usize; let global_port_id = self .registration @@ -351,8 +342,8 @@ where /// DROP SAFETY: No state that needs to be restored async fn wait_port_pending( &self, - controller: &mut C::Inner, - ) -> Result::BusError>> { + controller: &mut D::Inner, + ) -> Result::BusError>> { if self.state.lock().await.controller_state().fw_update_state.in_progress() { // Don't process events while firmware update is in progress debug!("Firmware update in progress, ignoring port events"); @@ -369,7 +360,7 @@ where // DROP SAFETY: Safe as long as `wait_port_event` is drop safe match select(controller.wait_port_event(), async { self.sw_status_event.wait().await; - Ok::<_, Error<::BusError>>(()) + Ok::<_, Error<::BusError>>(()) }) .await { @@ -382,7 +373,7 @@ where } /// Wait for the next event - pub async fn wait_next(&self) -> Result, Error<::BusError>> { + pub async fn wait_next(&self) -> Result, Error<::BusError>> { // This loop is to ensure that if we finish streaming events we go back to waiting for the next port event loop { let event = { @@ -401,7 +392,7 @@ where Either5::First(stream) => { let mut stream = stream?; if let Some((port_index, event)) = stream - .next::::BusError>, _, _>(async |port_index| { + .next::::BusError>, _, _>(async |port_index| { // Combine the event read from the controller with any software generated events // Acquire the locks first to centralize the awaits here let mut controller = self.controller.lock().await; @@ -473,10 +464,10 @@ where /// Process a port notification async fn process_port_notification<'b>( &self, - controller: &mut C::Inner, + controller: &mut D::Inner, port: LocalPortId, notification: PortNotificationSingle, - ) -> Result, Error<::BusError>> { + ) -> Result, Error<::BusError>> { match notification { PortNotificationSingle::Alert => { let ado = controller.get_pd_alert(port).await?; @@ -508,7 +499,7 @@ where pub async fn process_event<'b>( &self, event: Event<'b>, - ) -> Result, Error<::BusError>> { + ) -> Result, Error<::BusError>> { let mut controller = self.controller.lock().await; let mut state = self.state.lock().await; match event { @@ -518,13 +509,9 @@ where } Event::PowerPolicyCommand(EventPowerPolicyCommand { port, request }) => { let response = self - .process_power_command(&mut controller, state.deref_mut().deref_mut(), port, &request.command) + .process_power_command(&mut controller, state.deref_mut().deref_mut(), port, &request) .await; - Ok(Output::PowerPolicyCommand(OutputPowerPolicyCommand { - port, - request, - response, - })) + Ok(Output::PowerPolicyCommand(OutputPowerPolicyCommand { port, response })) } Event::ControllerCommand(request) => { let response = self @@ -554,7 +541,7 @@ where } /// Event loop finalize - pub async fn finalize<'b>(&self, output: Output<'b>) -> Result<(), Error<::BusError>> { + pub async fn finalize<'b>(&self, output: Output<'b>) -> Result<(), Error<::BusError>> { let mut state = self.state.lock().await; match output { @@ -567,9 +554,18 @@ where Output::PdAlert(OutputPdAlert { port, ado }) => { self.finalize_pd_alert(state.deref_mut().deref_mut(), port, ado) } - Output::Vdm(vdm) => self.finalize_vdm(state.deref_mut().deref_mut(), vdm).map_err(Error::Pd), - Output::PowerPolicyCommand(OutputPowerPolicyCommand { request, response, .. }) => { - request.respond(response); + Output::Vdm(vdm) => self + .finalize_vdm(state.deref_mut().deref_mut(), vdm) + .await + .map_err(Error::Pd), + Output::PowerPolicyCommand(OutputPowerPolicyCommand { port, response }) => { + self.power_proxy_receivers + .get(port.0 as usize) + .ok_or(Error::Pd(PdError::InvalidPort))? + .lock() + .await + .send(response) + .await; Ok(()) } Output::ControllerCommand(OutputControllerCommand { request, response }) => { @@ -595,13 +591,13 @@ where pub async fn process_and_finalize_event<'b>( &self, event: Event<'b>, - ) -> Result<(), Error<::BusError>> { + ) -> Result<(), Error<::BusError>> { let output = self.process_event(event).await?; self.finalize(output).await } /// Combined processing function - pub async fn process_next_event(&self) -> Result<(), Error<::BusError>> { + pub async fn process_next_event(&self) -> Result<(), Error<::BusError>> { let event = self.wait_next().await?; self.process_and_finalize_event(event).await } @@ -610,8 +606,7 @@ where pub async fn register( &'static self, controllers: &intrusive_list::IntrusiveList, - ) -> Result<(), Error<::BusError>> { - // TODO: Unify these devices? + ) -> Result<(), Error<::BusError>> { for device in self.registration.power_devices { policy::register_device(device).map_err(|_| { error!( @@ -645,7 +640,14 @@ where } } -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> Lockable for ControllerWrapper<'device, M, C, V> +impl< + 'device, + M: RawMutex, + C: Lockable, + S: event::Sender, + R: event::Receiver, + V: FwOfferValidator, +> Lockable for ControllerWrapper<'device, M, C, S, R, V> where ::Inner: Controller, { diff --git a/type-c-service/src/wrapper/pd.rs b/type-c-service/src/wrapper/pd.rs index f55bd8bbf..8eb62d697 100644 --- a/type-c-service/src/wrapper/pd.rs +++ b/type-c-service/src/wrapper/pd.rs @@ -2,6 +2,7 @@ use embassy_futures::yield_now; use embassy_sync::pubsub::WaitResult; use embassy_time::{Duration, Timer}; use embedded_services::debug; +use embedded_services::power::policy::device::State; use embedded_services::type_c::Cached; use embedded_services::type_c::controller::{InternalResponseData, Response}; use embedded_usb_pd::constants::{T_PS_TRANSITION_EPR_MS, T_PS_TRANSITION_SPR_MS}; @@ -9,13 +10,20 @@ use embedded_usb_pd::ucsi::{self, lpm}; use super::*; -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl< + 'device, + M: RawMutex, + D: Lockable, + S: event::Sender, + R: event::Receiver, + V: FwOfferValidator, +> ControllerWrapper<'device, M, D, S, R, V> where - ::Inner: Controller, + ::Inner: Controller, { async fn process_get_pd_alert( &self, - state: &mut dyn DynPortState<'_>, + state: &mut dyn DynPortState<'_, S>, local_port: LocalPortId, ) -> Result, PdError> { loop { @@ -45,7 +53,7 @@ where /// even for controllers that might not always broadcast sink ready events. pub(super) fn check_sink_ready_timeout( &self, - state: &mut dyn DynPortState<'_>, + state: &mut dyn DynPortState<'_, S>, status: &PortStatus, port: LocalPortId, new_contract: bool, @@ -115,34 +123,24 @@ where LocalPortId(port_index as u8) } - /// Set the maximum sink voltage for a port - pub async fn set_max_sink_voltage(&self, local_port: LocalPortId, voltage_mv: Option) -> Result<(), PdError> { - let mut controller = self.controller.lock().await; - let _ = self - .process_set_max_sink_voltage(&mut controller, local_port, voltage_mv) - .await?; - Ok(()) - } - /// Process a request to set the maximum sink voltage for a port async fn process_set_max_sink_voltage( &self, - controller: &mut C::Inner, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, local_port: LocalPortId, voltage_mv: Option, ) -> Result { - let power_device = self.get_power_device(local_port).ok_or(PdError::InvalidPort)?; - - let state = power_device.state().await; + let port_power = state + .port_power_mut() + .get_mut(local_port.0 as usize) + .ok_or(PdError::InvalidPort)?; + let state = port_power.state.state(); debug!("Port{}: Current state: {:#?}", local_port.0, state); - if let Ok(connected_consumer) = power_device.try_device_action::().await { + if matches!(state, State::ConnectedConsumer(_)) { debug!("Port{}: Set max sink voltage, connected consumer found", local_port.0); if voltage_mv.is_some() - && voltage_mv - < power_device - .consumer_capability() - .await - .map(|c| c.capability.voltage_mv) + && voltage_mv < port_power.state.consumer_capability().map(|c| c.capability.voltage_mv) { // New max voltage is lower than current consumer capability which will trigger a renegociation // So disconnect first @@ -150,7 +148,7 @@ where "Port{}: Disconnecting consumer before setting max sink voltage", local_port.0 ); - let _ = connected_consumer.disconnect().await; + port_power.sender.send(policy::RequestData::Disconnected).await; } } @@ -165,8 +163,8 @@ where async fn process_get_port_status( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, local_port: LocalPortId, cached: Cached, ) -> Result { @@ -192,8 +190,8 @@ where /// Handle a port command async fn process_port_command( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, command: &controller::PortCommand, ) -> Response<'static> { if state.controller_state().fw_update_state.in_progress() { @@ -272,7 +270,7 @@ where controller::PortCommandData::SetMaxSinkVoltage(voltage_mv) => { match self.registration.pd_controller.lookup_local_port(command.port) { Ok(local_port) => { - self.process_set_max_sink_voltage(controller, local_port, voltage_mv) + self.process_set_max_sink_voltage(controller, state, local_port, voltage_mv) .await } Err(e) => Err(e), @@ -401,8 +399,8 @@ where async fn process_controller_command( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, command: &controller::InternalCommandData, ) -> Response<'static> { if state.controller_state().fw_update_state.in_progress() { @@ -437,8 +435,8 @@ where /// Handle a PD controller command pub(super) async fn process_pd_command( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, command: &controller::Command, ) -> Response<'static> { match command { diff --git a/type-c-service/src/wrapper/power.rs b/type-c-service/src/wrapper/power.rs index 2d3682a1d..d1a7a6f32 100644 --- a/type-c-service/src/wrapper/power.rs +++ b/type-c-service/src/wrapper/power.rs @@ -1,51 +1,41 @@ //! Module contain power-policy related message handling -use core::future; +use core::pin::pin; +use embassy_futures::select::select_slice; use embedded_services::{ debug, - ipc::deferred, power::policy::{ ConsumerPowerCapability, ProviderPowerCapability, - device::{CommandData, InternalResponseData}, + device::{CommandData, InternalResponseData, ResponseData}, flags::PsuType, }, }; +use embedded_services::power::policy::Error as PowerError; +use embedded_services::power::policy::device::CommandData as PowerCommand; + use crate::wrapper::config::UnconstrainedSink; use super::*; -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl< + 'device, + M: RawMutex, + D: Lockable, + S: event::Sender, + R: event::Receiver, + V: FwOfferValidator, +> ControllerWrapper<'device, M, D, S, R, V> where - ::Inner: Controller, + ::Inner: Controller, { - /// Return the power device for the given port - pub fn get_power_device(&self, port: LocalPortId) -> Option<&policy::device::Device> { - self.registration.power_devices.get(port.0 as usize) - } - /// Handle a new contract as consumer pub(super) async fn process_new_consumer_contract( &self, - power: &policy::device::Device, + power: &mut PortPower, status: &PortStatus, - ) -> Result<(), Error<::BusError>> { + ) -> Result<(), Error<::BusError>> { info!("Process new consumer contract"); - - let current_state = power.state().await.kind(); - info!("current power state: {:?}", current_state); - - // Recover if we're not in the correct state - if status.is_connected() { - if let action::device::AnyState::Detached(state) = power.device_action().await { - warn!("Power device is detached, attempting to attach"); - if let Err(e) = state.attach().await { - error!("Error attaching power device: {:?}", e); - return PdError::Failed.into(); - } - } - } - let available_sink_contract = status.available_sink_contract.map(|c| { let mut c: ConsumerPowerCapability = c.into(); let unconstrained = match self.config.unconstrained_sink { @@ -58,89 +48,26 @@ where c }); - if let Ok(state) = power.try_device_action::().await { - if let Err(e) = state.notify_consumer_power_capability(available_sink_contract).await { - error!("Error setting power contract: {:?}", e); - return PdError::Failed.into(); - } - } else if let Ok(state) = power.try_device_action::().await { - if let Err(e) = state.notify_consumer_power_capability(available_sink_contract).await { - error!("Error setting power contract: {:?}", e); - return PdError::Failed.into(); - } - } else if let Ok(state) = power.try_device_action::().await { - if let Err(e) = state.notify_consumer_power_capability(available_sink_contract).await { - error!("Error setting power contract: {:?}", e); - return PdError::Failed.into(); - } - } else { - error!("Invalid mode"); - return PdError::InvalidMode.into(); - } - + power + .sender + .send(policy::RequestData::UpdatedConsumerCapability(available_sink_contract)) + .await; Ok(()) } /// Handle a new contract as provider pub(super) async fn process_new_provider_contract( &self, - power: &policy::device::Device, + power: &mut PortPower, status: &PortStatus, - ) -> Result<(), Error<::BusError>> { + ) -> Result<(), Error<::BusError>> { info!("Process New provider contract"); - - let current_state = power.state().await.kind(); - info!("current power state: {:?}", current_state); - - if let action::device::AnyState::ConnectedConsumer(state) = power.device_action().await { - info!("ConnectedConsumer"); - if let Err(e) = state.detach().await { - info!("Error detaching power device: {:?}", e); - return PdError::Failed.into(); - } - } - - // Recover if we're not in the correct state - if status.is_connected() { - if let action::device::AnyState::Detached(state) = power.device_action().await { - warn!("Power device is detached, attempting to attach"); - if let Err(e) = state.attach().await { - error!("Error attaching power device: {:?}", e); - return PdError::Failed.into(); - } - } - } - - let contract = status.available_source_contract.map(|c| { - let mut c: ProviderPowerCapability = c.into(); - c.flags.set_psu_type(PsuType::TypeC); - c - }); - if let Ok(state) = power.try_device_action::().await { - if let Some(contract) = contract { - if let Err(e) = state.request_provider_power_capability(contract).await { - error!("Error setting power contract: {:?}", e); - return PdError::Failed.into(); - } - } - } else if let Ok(state) = power.try_device_action::().await { - if let Some(contract) = contract { - if let Err(e) = state.request_provider_power_capability(contract).await { - error!("Error setting power contract: {:?}", e); - return PdError::Failed.into(); - } - } else { - // No longer need to source, so disconnect - if let Err(e) = state.disconnect().await { - error!("Error setting power contract: {:?}", e); - return PdError::Failed.into(); - } - } - } else { - error!("Invalid mode"); - return PdError::InvalidMode.into(); - } - + power + .sender + .send(policy::RequestData::RequestedProviderCapability( + status.available_source_contract.map(ProviderPowerCapability::from), + )) + .await; Ok(()) } @@ -148,18 +75,12 @@ where async fn process_disconnect( &self, port: LocalPortId, - controller: &mut C::Inner, - power: &policy::device::Device, - ) -> Result<(), Error<::BusError>> { - let state = power.state().await.kind(); - if state == StateKind::ConnectedConsumer { - info!("Port{}: Disconnect from ConnectedConsumer", port.0); - if controller.enable_sink_path(port, false).await.is_err() { - error!("Error disabling sink path"); - return PdError::Failed.into(); - } + controller: &mut D::Inner, + ) -> Result<(), Error<::BusError>> { + if controller.enable_sink_path(port, false).await.is_err() { + error!("Error disabling sink path"); + return PdError::Failed.into(); } - Ok(()) } @@ -168,8 +89,8 @@ where &self, port: LocalPortId, capability: ProviderPowerCapability, - _controller: &mut C::Inner, - ) -> Result<(), Error<::BusError>> { + _controller: &mut D::Inner, + ) -> Result<(), Error<::BusError>> { info!("Port{}: Connect as provider: {:#?}", port.0, capability); // TODO: double check explicit contract handling Ok(()) @@ -179,22 +100,23 @@ where /// /// Returns (local port ID, deferred request) /// DROP SAFETY: Call to a select over drop safe futures - pub(super) async fn wait_power_command( - &self, - ) -> ( - LocalPortId, - deferred::Request<'_, GlobalRawMutex, CommandData, InternalResponseData>, - ) { - let futures: [_; MAX_SUPPORTED_PORTS] = from_fn(|i| async move { - if let Some(device) = self.registration.power_devices.get(i) { - device.receive().await - } else { - future::pending().await + pub(super) async fn wait_power_command(&self) -> (LocalPortId, CommandData) { + let mut futures = heapless::Vec::<_, MAX_SUPPORTED_PORTS>::new(); + for receiver in self.power_proxy_receivers { + if futures + .push(async { + let mut lock = receiver.lock().await; + lock.receive().await + }) + .is_err() + { + error!("Futures vec overflow"); } - }); + } + // DROP SAFETY: Select over drop safe futures - let (request, local_id) = select_array(futures).await; - trace!("Power command: device{} {:#?}", local_id, request.command); + let (request, local_id) = select_slice(pin!(futures.as_mut_slice())).await; + trace!("Power command: device{} {:#?}", local_id, request); (LocalPortId(local_id as u8), request) } @@ -202,50 +124,42 @@ where /// Returns no error because this is a top-level function pub(super) async fn process_power_command( &self, - controller: &mut C::Inner, - state: &mut dyn DynPortState<'_>, + controller: &mut D::Inner, + state: &mut dyn DynPortState<'_, S>, port: LocalPortId, command: &CommandData, ) -> InternalResponseData { trace!("Processing power command: device{} {:#?}", port.0, command); if state.controller_state().fw_update_state.in_progress() { debug!("Port{}: Firmware update in progress", port.0); - return Err(policy::Error::Busy); + return Err(PowerError::Busy); } - let power = match self.get_power_device(port) { - Some(power) => power, - None => { - error!("Port{}: Error getting power device for port", port.0); - return Err(policy::Error::InvalidDevice); - } - }; - match command { - policy::device::CommandData::ConnectAsConsumer(capability) => { + PowerCommand::ConnectAsConsumer(capability) => { info!( "Port{}: Connect as consumer: {:?}, enable input switch", port.0, capability ); if controller.enable_sink_path(port, true).await.is_err() { error!("Error enabling sink path"); - return Err(policy::Error::Failed); + return Err(PowerError::Failed); } } - policy::device::CommandData::ConnectAsProvider(capability) => { + PowerCommand::ConnectAsProvider(capability) => { if self.process_connect_as_provider(port, *capability, controller).is_err() { error!("Error processing connect provider"); - return Err(policy::Error::Failed); + return Err(PowerError::Failed); } } - policy::device::CommandData::Disconnect => { - if self.process_disconnect(port, controller, power).await.is_err() { + PowerCommand::Disconnect => { + if self.process_disconnect(port, controller).await.is_err() { error!("Error processing disconnect"); - return Err(policy::Error::Failed); + return Err(PowerError::Failed); } } } - Ok(policy::device::ResponseData::Complete) + Ok(ResponseData::Complete) } } diff --git a/type-c-service/src/wrapper/proxy.rs b/type-c-service/src/wrapper/proxy.rs new file mode 100644 index 000000000..252247e88 --- /dev/null +++ b/type-c-service/src/wrapper/proxy.rs @@ -0,0 +1,105 @@ +use embassy_sync::blocking_mutex::raw::RawMutex; +use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender}; +use embedded_services::power; +use embedded_services::power::policy::device::{ + CommandData as PolicyCommandData, DeviceTrait, InternalResponseData as PolicyResponseData, +}; + +pub struct PowerProxyChannel { + command_channel: Channel, + response_channel: Channel, +} + +impl PowerProxyChannel { + pub fn new() -> Self { + Self { + command_channel: Channel::new(), + response_channel: Channel::new(), + } + } + + pub fn get_device(&self) -> PowerProxyDevice<'_> { + PowerProxyDevice { + sender: self.command_channel.dyn_sender(), + receiver: self.response_channel.dyn_receiver(), + } + } + + pub fn get_receiver(&self) -> PowerProxyReceiver<'_> { + PowerProxyReceiver { + receiver: self.command_channel.dyn_receiver(), + sender: self.response_channel.dyn_sender(), + } + } +} + +pub struct PowerProxyReceiver<'a> { + sender: DynamicSender<'a, PolicyResponseData>, + receiver: DynamicReceiver<'a, PolicyCommandData>, +} + +impl<'a> PowerProxyReceiver<'a> { + pub fn new( + receiver: DynamicReceiver<'a, PolicyCommandData>, + sender: DynamicSender<'a, PolicyResponseData>, + ) -> Self { + Self { receiver, sender } + } + + pub async fn receive(&mut self) -> PolicyCommandData { + self.receiver.receive().await + } + + pub async fn send(&mut self, response: PolicyResponseData) { + self.sender.send(response).await; + } +} + +pub struct PowerProxyDevice<'a> { + sender: DynamicSender<'a, PolicyCommandData>, + receiver: DynamicReceiver<'a, PolicyResponseData>, +} + +impl<'a> PowerProxyDevice<'a> { + pub fn new( + sender: DynamicSender<'a, PolicyCommandData>, + receiver: DynamicReceiver<'a, PolicyResponseData>, + ) -> Self { + Self { sender, receiver } + } + + async fn execute(&mut self, command: PolicyCommandData) -> PolicyResponseData { + self.sender.send(command).await; + self.receiver.receive().await + } +} + +impl<'a> DeviceTrait for PowerProxyDevice<'a> { + async fn disconnect(&mut self) -> Result<(), power::policy::Error> { + self.execute(PolicyCommandData::Disconnect).await?.complete_or_err() + } + + async fn connect_provider( + &mut self, + capability: power::policy::ProviderPowerCapability, + ) -> Result<(), power::policy::Error> { + self.execute(PolicyCommandData::ConnectAsProvider(capability)) + .await? + .complete_or_err() + } + + async fn connect_consumer( + &mut self, + capability: power::policy::ConsumerPowerCapability, + ) -> Result<(), power::policy::Error> { + self.execute(PolicyCommandData::ConnectAsConsumer(capability)) + .await? + .complete_or_err() + } +} + +impl Default for PowerProxyChannel { + fn default() -> Self { + Self::new() + } +} diff --git a/type-c-service/src/wrapper/vdm.rs b/type-c-service/src/wrapper/vdm.rs index 029b7f7d1..844d8bfd1 100644 --- a/type-c-service/src/wrapper/vdm.rs +++ b/type-c-service/src/wrapper/vdm.rs @@ -1,5 +1,7 @@ use embassy_sync::blocking_mutex::raw::RawMutex; use embedded_services::{ + event, + power::policy::policy, sync::Lockable, trace, type_c::{ @@ -13,17 +15,24 @@ use crate::wrapper::{DynPortState, message::vdm::OutputKind}; use super::{ControllerWrapper, FwOfferValidator, message::vdm::Output}; -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl< + 'device, + M: RawMutex, + D: Lockable, + S: event::Sender, + R: event::Receiver, + V: FwOfferValidator, +> ControllerWrapper<'device, M, D, S, R, V> where - ::Inner: Controller, + ::Inner: Controller, { /// Process a VDM event by retrieving the relevant VDM data from the `controller` for the appropriate `port`. pub(super) async fn process_vdm_event( &self, - controller: &mut C::Inner, + controller: &mut D::Inner, port: LocalPortId, event: VdmNotification, - ) -> Result::BusError>> { + ) -> Result::BusError>> { trace!("Processing VDM event: {:?} on port {}", event, port.0); let kind = match event { VdmNotification::Entered => OutputKind::Entered(controller.get_other_vdm(port).await?), @@ -36,7 +45,11 @@ where } /// Finalize a VDM output by notifying the service. - pub(super) fn finalize_vdm(&self, state: &mut dyn DynPortState<'_>, output: Output) -> Result<(), PdError> { + pub(super) async fn finalize_vdm( + &self, + state: &mut dyn DynPortState<'_, S>, + output: Output, + ) -> Result<(), PdError> { trace!("Finalizing VDM output: {:?}", output); let Output { port, kind } = output; let global_port_id = self.registration.pd_controller.lookup_global_port(port)?;