From adc2f92e5aaf66813909edaf11b40e34b62283a8 Mon Sep 17 00:00:00 2001 From: matteotullo Date: Mon, 26 Jan 2026 16:02:32 -0800 Subject: [PATCH] Power policy refactor --- .../src/power/policy/action/device.rs | 78 +++--- .../src/power/policy/action/policy.rs | 52 ++-- embedded-service/src/power/policy/device.rs | 35 ++- embedded-service/src/power/policy/mod.rs | 2 +- embedded-service/src/power/policy/policy.rs | 246 ++++++++---------- examples/rt685s-evk/src/bin/type_c.rs | 38 ++- examples/rt685s-evk/src/bin/type_c_cfu.rs | 40 ++- examples/rt685s-evk/src/lib.rs | 18 ++ examples/std/src/bin/power_policy.rs | 58 +++-- examples/std/src/bin/type_c/basic.rs | 29 ++- examples/std/src/bin/type_c/external.rs | 24 +- examples/std/src/bin/type_c/service.rs | 40 ++- examples/std/src/bin/type_c/ucsi.rs | 49 +++- examples/std/src/bin/type_c/unconstrained.rs | 67 +++-- .../std/src/lib/type_c/mock_controller.rs | 11 +- examples/std/src/lib/type_c/mod.rs | 18 ++ power-policy-service/src/consumer.rs | 12 +- power-policy-service/src/lib.rs | 20 +- power-policy-service/src/provider.rs | 8 +- power-policy-service/src/task.rs | 75 +++++- type-c-service/src/service/mod.rs | 7 +- type-c-service/src/task.rs | 29 ++- type-c-service/src/wrapper/backing.rs | 49 ++-- type-c-service/src/wrapper/cfu.rs | 3 +- type-c-service/src/wrapper/dp.rs | 3 +- type-c-service/src/wrapper/mod.rs | 19 +- type-c-service/src/wrapper/pd.rs | 3 +- type-c-service/src/wrapper/power.rs | 11 +- type-c-service/src/wrapper/vdm.rs | 3 +- 29 files changed, 667 insertions(+), 380 deletions(-) diff --git a/embedded-service/src/power/policy/action/device.rs b/embedded-service/src/power/policy/action/device.rs index 01d0d6c36..b08a7681f 100644 --- a/embedded-service/src/power/policy/action/device.rs +++ b/embedded-service/src/power/policy/action/device.rs @@ -4,24 +4,24 @@ use crate::power::policy::{ConsumerPowerCapability, Error, ProviderPowerCapabili use crate::{info, trace}; /// Device state machine control -pub struct Device<'a, S: Kind> { - device: &'a device::Device, +pub struct Device<'a, S: Kind, const N: usize> { + device: &'a device::Device, _state: core::marker::PhantomData, } /// Enum to contain any state -pub enum AnyState<'a> { +pub enum AnyState<'a, const N: usize> { /// Detached - Detached(Device<'a, Detached>), + Detached(Device<'a, Detached, N>), /// Idle - Idle(Device<'a, Idle>), + Idle(Device<'a, Idle, N>), /// Connected Consumer - ConnectedConsumer(Device<'a, ConnectedConsumer>), + ConnectedConsumer(Device<'a, ConnectedConsumer, N>), /// Connected Provider - ConnectedProvider(Device<'a, ConnectedProvider>), + ConnectedProvider(Device<'a, ConnectedProvider, N>), } -impl AnyState<'_> { +impl AnyState<'_, N> { /// Return the kind of the contained state pub fn kind(&self) -> StateKind { match self { @@ -33,9 +33,9 @@ impl AnyState<'_> { } } -impl<'a, S: Kind> Device<'a, S> { +impl<'a, S: Kind, const N: usize> Device<'a, S, N> { /// Create a new state machine - pub(crate) fn new(device: &'a device::Device) -> Self { + pub(crate) fn new(device: &'a device::Device) -> Self { Self { device, _state: core::marker::PhantomData, @@ -43,12 +43,14 @@ impl<'a, S: Kind> Device<'a, S> { } /// Detach the device - pub async fn detach(self) -> Result, Error> { + 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) + self.device + .context_ref + .send_request(self.device.id(), policy::RequestData::NotifyDetached) .await? .complete_or_err()?; Ok(Device::new(self.device)) @@ -60,7 +62,9 @@ impl<'a, S: Kind> Device<'a, S> { 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) + self.device + .context_ref + .send_request(self.device.id(), policy::RequestData::NotifyDisconnect) .await? .complete_or_err() } @@ -76,12 +80,14 @@ impl<'a, S: Kind> Device<'a, S> { capability ); self.device.update_consumer_capability(capability).await; - policy::send_request( - self.device.id(), - policy::RequestData::NotifyConsumerCapability(capability), - ) - .await? - .complete_or_err() + self.device + .context_ref + .send_request( + self.device.id(), + policy::RequestData::NotifyConsumerCapability(capability), + ) + .await? + .complete_or_err() } /// Request the given power from the power policy service @@ -97,29 +103,33 @@ impl<'a, S: Kind> Device<'a, S> { 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()?; + self.device + .context_ref + .send_request( + self.device.id(), + policy::RequestData::RequestProviderCapability(capability), + ) + .await? + .complete_or_err()?; Ok(()) } } -impl<'a> Device<'a, Detached> { +impl<'a, const N: usize> Device<'a, Detached, N> { /// Attach the device - pub async fn attach(self) -> Result, Error> { + 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) + self.device + .context_ref + .send_request(self.device.id(), policy::RequestData::NotifyAttached) .await? .complete_or_err()?; Ok(Device::new(self.device)) } } -impl Device<'_, Idle> { +impl Device<'_, Idle, N> { /// Notify the power policy service of an updated consumer power capability pub async fn notify_consumer_power_capability( &self, @@ -134,9 +144,9 @@ impl Device<'_, Idle> { } } -impl<'a> Device<'a, ConnectedConsumer> { +impl<'a, const N: usize> Device<'a, ConnectedConsumer, N> { /// Disconnect this device - pub async fn disconnect(self) -> Result, Error> { + pub async fn disconnect(self) -> Result, Error> { self.disconnect_internal().await?; Ok(Device::new(self.device)) } @@ -144,15 +154,16 @@ impl<'a> Device<'a, ConnectedConsumer> { /// 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> { +impl<'a, const N: usize> Device<'a, ConnectedProvider, N> { /// Disconnect this device - pub async fn disconnect(self) -> Result, Error> { + pub async fn disconnect(self) -> Result, Error> { self.disconnect_internal().await?; Ok(Device::new(self.device)) } @@ -165,6 +176,7 @@ impl<'a> Device<'a, ConnectedProvider> { /// 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/policy.rs b/embedded-service/src/power/policy/action/policy.rs index 0559421fc..540ff762d 100644 --- a/embedded-service/src/power/policy/action/policy.rs +++ b/embedded-service/src/power/policy/action/policy.rs @@ -9,24 +9,24 @@ use crate::{error, info}; const DEFAULT_TIMEOUT: Duration = Duration::from_millis(5000); /// Policy state machine control -pub struct Policy<'a, S: Kind> { - device: &'a device::Device, +pub struct Policy<'a, S: Kind, const N: usize> { + device: &'a device::Device, _state: core::marker::PhantomData, } /// Enum to contain any state -pub enum AnyState<'a> { +pub enum AnyState<'a, const N: usize> { /// Detached - Detached(Policy<'a, Detached>), + Detached(Policy<'a, Detached, N>), /// Idle - Idle(Policy<'a, Idle>), + Idle(Policy<'a, Idle, N>), /// Connected Consumer - ConnectedConsumer(Policy<'a, ConnectedConsumer>), + ConnectedConsumer(Policy<'a, ConnectedConsumer, N>), /// Connected Provider - ConnectedProvider(Policy<'a, ConnectedProvider>), + ConnectedProvider(Policy<'a, ConnectedProvider, N>), } -impl AnyState<'_> { +impl AnyState<'_, N> { /// Return the kind of the contained state pub fn kind(&self) -> StateKind { match self { @@ -38,9 +38,9 @@ impl AnyState<'_> { } } -impl<'a, S: Kind> Policy<'a, S> { +impl<'a, S: Kind, const N: usize> Policy<'a, S, N> { /// Create a new state machine - pub(crate) fn new(device: &'a device::Device) -> Self { + pub(crate) fn new(device: &'a device::Device) -> Self { Self { device, _state: core::marker::PhantomData, @@ -97,14 +97,14 @@ impl<'a, S: Kind> Policy<'a, S> { } // The policy can do nothing when no device is attached -impl Policy<'_, Detached> {} +impl Policy<'_, Detached, N> {} -impl<'a> Policy<'a, Idle> { +impl<'a, const N: usize> Policy<'a, Idle, N> { /// Connect this device as a consumer pub async fn connect_as_consumer_no_timeout( self, capability: ConsumerPowerCapability, - ) -> Result, Error> { + ) -> Result, Error> { info!("Device {} connecting as consumer", self.device.id().0); self.device @@ -122,7 +122,7 @@ impl<'a> Policy<'a, Idle> { pub async fn connect_consumer( self, capability: ConsumerPowerCapability, - ) -> Result, Error> { + ) -> Result, Error> { match with_timeout(DEFAULT_TIMEOUT, self.connect_as_consumer_no_timeout(capability)).await { Ok(r) => r, Err(TimeoutError) => Err(Error::Timeout), @@ -133,7 +133,7 @@ impl<'a> Policy<'a, Idle> { pub async fn connect_provider_no_timeout( self, capability: ProviderPowerCapability, - ) -> Result, Error> { + ) -> Result, Error> { self.connect_as_provider_internal_no_timeout(capability) .await .map(|_| Policy::new(self.device)) @@ -143,30 +143,30 @@ impl<'a> Policy<'a, Idle> { pub async fn connect_provider( self, capability: ProviderPowerCapability, - ) -> Result, Error> { + ) -> Result, Error> { self.connect_provider_internal(capability) .await .map(|_| Policy::new(self.device)) } } -impl<'a> Policy<'a, ConnectedConsumer> { +impl<'a, const N: usize> Policy<'a, ConnectedConsumer, N> { /// Disconnect this device - pub async fn disconnect_no_timeout(self) -> Result, Error> { + 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> { + pub async fn disconnect(self) -> Result, Error> { self.disconnect_internal().await.map(|_| Policy::new(self.device)) } } -impl<'a> Policy<'a, ConnectedProvider> { +impl<'a, const N: usize> Policy<'a, ConnectedProvider, N> { /// Disconnect this device - pub async fn disconnect_no_timeout(self) -> Result, Error> { + 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); @@ -175,7 +175,7 @@ impl<'a> Policy<'a, ConnectedProvider> { } /// Disconnect this device - pub async fn disconnect(self) -> Result, Error> { + 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), @@ -186,7 +186,7 @@ impl<'a> Policy<'a, ConnectedProvider> { pub async fn connect_as_consumer_no_timeout( self, capability: ConsumerPowerCapability, - ) -> Result, Error> { + ) -> Result, Error> { info!("Device {} connecting as consumer", self.device.id().0); self.device @@ -204,7 +204,7 @@ impl<'a> Policy<'a, ConnectedProvider> { pub async fn connect_consumer( self, capability: ConsumerPowerCapability, - ) -> Result, Error> { + ) -> Result, Error> { match with_timeout(DEFAULT_TIMEOUT, self.connect_as_consumer_no_timeout(capability)).await { Ok(r) => r, Err(TimeoutError) => Err(Error::Timeout), @@ -215,7 +215,7 @@ impl<'a> Policy<'a, ConnectedProvider> { pub async fn connect_provider_no_timeout( &self, capability: ProviderPowerCapability, - ) -> Result, Error> { + ) -> Result, Error> { self.connect_as_provider_internal_no_timeout(capability) .await .map(|_| Policy::new(self.device)) @@ -225,7 +225,7 @@ impl<'a> Policy<'a, ConnectedProvider> { pub async fn connect_provider( &self, capability: ProviderPowerCapability, - ) -> Result, Error> { + ) -> Result, Error> { self.connect_provider_internal(capability) .await .map(|_| Policy::new(self.device)) diff --git a/embedded-service/src/power/policy/device.rs b/embedded-service/src/power/policy/device.rs index 23a6cc052..45fe2fcdd 100644 --- a/embedded-service/src/power/policy/device.rs +++ b/embedded-service/src/power/policy/device.rs @@ -5,6 +5,7 @@ use embassy_sync::mutex::Mutex; use super::{DeviceId, Error, action}; use crate::ipc::deferred; +use crate::power::policy::policy::Context; use crate::power::policy::{ConsumerPowerCapability, ProviderPowerCapability}; use crate::{GlobalRawMutex, intrusive_list}; @@ -111,7 +112,7 @@ pub struct Response { } /// Device struct -pub struct Device { +pub struct Device { /// Intrusive list node node: intrusive_list::Node, /// Device ID @@ -120,11 +121,13 @@ pub struct Device { state: Mutex, /// Command channel command: deferred::Channel, + /// Reference back to the power policy context to send messages + pub(crate) context_ref: &'static Context, } -impl Device { +impl Device { /// Create a new device - pub fn new(id: DeviceId) -> Self { + pub fn new(id: DeviceId, context_ref: &'static Context) -> Self { Self { node: intrusive_list::Node::uninit(), id, @@ -134,6 +137,7 @@ impl Device { requested_provider_capability: None, }), command: deferred::Channel::new(), + context_ref, } } @@ -209,7 +213,9 @@ impl Device { } /// Try to provide access to the device actions for the given state - pub async fn try_device_action(&self) -> Result, Error> { + 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)); @@ -218,7 +224,7 @@ impl Device { } /// Provide access to the current device state - pub async fn device_action(&self) -> action::device::AnyState<'_> { + pub async fn device_action(&self) -> action::device::AnyState<'_, CHANNEL_SIZE> { 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)), @@ -233,7 +239,9 @@ impl Device { /// 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> { + 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)); @@ -243,7 +251,7 @@ impl Device { /// Provide access to the current policy actions /// Implemented here for lifetime reasons - pub(super) async fn policy_action(&self) -> action::policy::AnyState<'_> { + pub(super) async fn policy_action(&self) -> action::policy::AnyState<'_, CHANNEL_SIZE> { 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)), @@ -257,7 +265,7 @@ impl Device { } /// Detach the device, this action is available in all states - pub async fn detach(&self) -> Result, Error> { + 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, @@ -267,20 +275,21 @@ impl Device { } } -impl intrusive_list::NodeContainer for Device { +// This must be static due to a bound on IntrusiveNode needing a static reference to Any +impl intrusive_list::NodeContainer for Device { fn get_node(&self) -> &crate::Node { &self.node } } /// Trait for any container that holds a device -pub trait DeviceContainer { +pub trait DeviceContainer { /// Get the underlying device struct - fn get_power_policy_device(&self) -> &Device; + fn get_power_policy_device(&self) -> &Device; } -impl DeviceContainer for Device { - fn get_power_policy_device(&self) -> &Device { +impl DeviceContainer for Device { + fn get_power_policy_device(&self) -> &Device { self } } diff --git a/embedded-service/src/power/policy/mod.rs b/embedded-service/src/power/policy/mod.rs index b925f860b..3b1d4eb76 100644 --- a/embedded-service/src/power/policy/mod.rs +++ b/embedded-service/src/power/policy/mod.rs @@ -5,7 +5,7 @@ pub mod device; pub mod flags; pub mod policy; -pub use policy::{init, register_device}; +pub use policy::init; use crate::power::policy::charger::ChargerError; diff --git a/embedded-service/src/power/policy/policy.rs b/embedded-service/src/power/policy/policy.rs index 3e3d694bb..25cf25976 100644 --- a/embedded-service/src/power/policy/policy.rs +++ b/embedded-service/src/power/policy/policy.rs @@ -1,5 +1,4 @@ //! Context for any power policy implementations -use core::sync::atomic::{AtomicBool, Ordering}; use crate::GlobalRawMutex; use crate::broadcaster::immediate as broadcaster; @@ -12,9 +11,6 @@ use super::{DeviceId, Error, action, 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))] @@ -72,212 +68,200 @@ pub struct Response { type InternalResponseData = Result; /// Power policy context -struct Context { +pub struct Context { /// Registered devices - devices: intrusive_list::IntrusiveList, + power_devices: intrusive_list::IntrusiveList, /// Policy request policy_request: Channel, /// Policy response policy_response: Channel, /// Registered chargers - chargers: intrusive_list::IntrusiveList, + charger_devices: intrusive_list::IntrusiveList, /// Message broadcaster broadcaster: broadcaster::Immediate, } -impl Context { - const fn new() -> Self { +impl Default for Context { + fn default() -> Self { + Self::new() + } +} + +impl Context { + /// Construct a new power policy Context + pub const fn new() -> Self { Self { - devices: intrusive_list::IntrusiveList::new(), - chargers: intrusive_list::IntrusiveList::new(), + power_devices: intrusive_list::IntrusiveList::new(), + charger_devices: intrusive_list::IntrusiveList::new(), policy_request: Channel::new(), policy_response: Channel::new(), broadcaster: broadcaster::Immediate::new(), } } -} - -static CONTEXT: Context = Context::new(); -/// Init power policy service -pub fn init() {} + /// Register a device with the power policy service + pub fn register_device( + &self, + device: &'static impl device::DeviceContainer, + ) -> Result<(), intrusive_list::Error> { + let device = device.get_power_policy_device(); + if self.get_device(device.id()).is_ok() { + return Err(intrusive_list::Error::NodeAlreadyInList); + } -/// Register a device with the power policy service -pub fn register_device(device: &'static impl device::DeviceContainer) -> Result<(), intrusive_list::Error> { - let device = device.get_power_policy_device(); - if get_device(device.id()).is_some() { - return Err(intrusive_list::Error::NodeAlreadyInList); + self.power_devices.push(device) } - CONTEXT.devices.push(device) -} + /// Register a charger with the power policy service + pub fn register_charger( + &self, + device: &'static impl charger::ChargerContainer, + ) -> Result<(), intrusive_list::Error> { + let device = device.get_charger(); + if self.get_charger(device.id()).is_ok() { + return Err(intrusive_list::Error::NodeAlreadyInList); + } -/// Register a charger with the power policy service -pub fn register_charger(device: &'static impl charger::ChargerContainer) -> Result<(), intrusive_list::Error> { - let device = device.get_charger(); - if get_charger(device.id()).is_some() { - return Err(intrusive_list::Error::NodeAlreadyInList); + self.charger_devices.push(device) } - CONTEXT.chargers.push(device) -} - -/// Find a device by its ID -fn get_device(id: DeviceId) -> Option<&'static device::Device> { - for device in &CONTEXT.devices { - if let Some(data) = device.data::() { - if data.id() == id { - return Some(data); + /// Get a device by its ID + pub fn get_device(&self, id: DeviceId) -> Result<&'static device::Device, Error> { + for device in &self.power_devices { + if let Some(data) = device.data::>() { + if data.id() == id { + return Ok(data); + } + } else { + error!("Non-device located in devices list"); } - } else { - error!("Non-device located in devices list"); } - } - None -} + Err(Error::InvalidDevice) + } -/// Returns the total amount of power that is being supplied to external devices -pub async fn compute_total_provider_power_mw() -> u32 { - let mut total = 0; - 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(); + /// Returns the total amount of power that is being supplied to external devices + pub async fn compute_total_provider_power_mw(&self) -> u32 { + let mut total = 0; + for device in self.power_devices.iter_only::>() { + if let Some(capability) = device.provider_capability().await { + if device.is_provider().await { + total += capability.capability.max_power_mw(); + } } } + total } - total -} -/// Find a device by its ID -fn get_charger(id: charger::ChargerId) -> Option<&'static charger::Device> { - for charger in &CONTEXT.chargers { - if let Some(data) = charger.data::() { - if data.id() == id { - return Some(data); + /// Get a charger by its ID + pub fn get_charger(&self, id: charger::ChargerId) -> Result<&'static charger::Device, Error> { + for charger in &self.charger_devices { + if let Some(data) = charger.data::() { + if data.id() == id { + return Ok(data); + } + } else { + error!("Non-device located in charger list"); } - } else { - error!("Non-device located in charger list"); } - } - - 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 -} + Err(Error::InvalidDevice) + } -/// Initialize chargers in hardware -pub async fn init_chargers() -> ChargerResponse { - for charger in &CONTEXT.chargers { - if let Some(data) = charger.data::() { - data.execute_command(charger::PolicyEvent::InitRequest) - .await - .inspect_err(|e| error!("Charger {:?} failed InitRequest: {:?}", data.id(), e))?; - } + /// Convenience function to send a request to the power policy service + pub(super) async fn send_request(&self, from: DeviceId, request: RequestData) -> Result { + self.policy_request + .send(Request { + id: from, + data: request, + }) + .await; + self.policy_response.receive().await } - Ok(Ack) -} -/// Check if charger hardware is ready for communications. -pub async fn check_chargers_ready() -> ChargerResponse { - for charger in &CONTEXT.chargers { - if let Some(data) = charger.data::() { - data.execute_command(charger::PolicyEvent::CheckReady) - .await - .inspect_err(|e| error!("Charger {:?} failed CheckReady: {:?}", data.id(), e))?; + /// Initialize chargers in hardware + pub async fn init_chargers(&self) -> ChargerResponse { + for charger in &self.charger_devices { + if let Some(data) = charger.data::() { + data.execute_command(charger::PolicyEvent::InitRequest) + .await + .inspect_err(|e| error!("Charger {:?} failed InitRequest: {:?}", data.id(), e))?; + } } + Ok(Ack) } - Ok(Ack) -} -/// Register a message receiver for power policy messages -pub fn register_message_receiver( - receiver: &'static broadcaster::Receiver<'_, CommsMessage>, -) -> intrusive_list::Result<()> { - CONTEXT.broadcaster.register_receiver(receiver) -} - -/// Singleton struct to give access to the power policy context -pub struct ContextToken(()); - -impl ContextToken { - /// Create a new context token, returning None if this function has been called before - pub fn create() -> Option { - static INIT: AtomicBool = AtomicBool::new(false); - if INIT.load(Ordering::SeqCst) { - return None; + /// Check if charger hardware is ready for communications. + pub async fn check_chargers_ready(&self) -> ChargerResponse { + for charger in &self.charger_devices { + if let Some(data) = charger.data::() { + data.execute_command(charger::PolicyEvent::CheckReady) + .await + .inspect_err(|e| error!("Charger {:?} failed CheckReady: {:?}", data.id(), e))?; + } } + Ok(Ack) + } - INIT.store(true, Ordering::SeqCst); - Some(ContextToken(())) + /// Register a message receiver for power policy messages + pub fn register_message_receiver( + &self, + receiver: &'static broadcaster::Receiver<'_, CommsMessage>, + ) -> intrusive_list::Result<()> { + self.broadcaster.register_receiver(receiver) } /// Initialize Policy charger devices - pub async fn init() -> Result<(), Error> { + pub async fn init(&self) -> Result<(), Error> { // Check if the chargers are powered and able to communicate - check_chargers_ready().await?; + self.check_chargers_ready().await?; // Initialize chargers - init_chargers().await?; + self.init_chargers().await?; Ok(()) } /// Wait for a power policy request pub async fn wait_request(&self) -> Request { - CONTEXT.policy_request.receive().await + self.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> { - get_device(id).ok_or(Error::InvalidDevice) + self.policy_response.send(response).await } /// Provides access to the device list pub fn devices(&self) -> &intrusive_list::IntrusiveList { - &CONTEXT.devices - } - - /// Get a charger by its ID - pub fn get_charger(&self, id: charger::ChargerId) -> Result<&'static charger::Device, Error> { - get_charger(id).ok_or(Error::InvalidDevice) + &self.power_devices } /// Provides access to the charger list pub fn chargers(&self) -> &intrusive_list::IntrusiveList { - &CONTEXT.chargers + &self.charger_devices } /// 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> { + ) -> 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> { + 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; + self.broadcaster.broadcast(message).await; } } + +/// Init power policy service +pub fn init() {} diff --git a/examples/rt685s-evk/src/bin/type_c.rs b/examples/rt685s-evk/src/bin/type_c.rs index 3d46d274d..8264a9954 100644 --- a/examples/rt685s-evk/src/bin/type_c.rs +++ b/examples/rt685s-evk/src/bin/type_c.rs @@ -32,6 +32,7 @@ const PORT0_ID: GlobalPortId = GlobalPortId(0); const PORT1_ID: GlobalPortId = GlobalPortId(1); const PORT0_PWR_ID: PowerId = PowerId(0); const PORT1_PWR_ID: PowerId = PowerId(1); +const POLICY_CHANNEL_SIZE: usize = 1; bind_interrupts!(struct Irqs { FLEXCOMM2 => embassy_imxrt::i2c::InterruptHandler; @@ -49,7 +50,7 @@ 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>, Validator, POLICY_CHANNEL_SIZE>; type Controller<'a> = tps6699x::controller::Controller>; type Interrupt<'a> = tps6699x::Interrupt<'a, GlobalRawMutex, BusDevice<'a>>; @@ -68,10 +69,14 @@ 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()) - .await - .expect("Failed to start power policy service task"); +async fn power_policy_service_task(policy: &'static power_policy_service::PowerPolicy) { + power_policy_service::task::task( + policy, + None::<[&rt685s_evk_example::DummyPowerDevice; 0]>, + None::<[&rt685s_evk_example::DummyCharger; 0]>, + ) + .await + .expect("Failed to start power policy service task"); } #[embassy_executor::task] @@ -79,6 +84,7 @@ async fn service_task( controller_context: &'static embedded_services::type_c::controller::Context, controllers: &'static IntrusiveList, wrappers: [&'static Wrapper<'static>; NUM_PD_CONTROLLERS], + power_policy_context: &'static embedded_services::power::policy::policy::Context, ) { info!("Starting type-c task"); @@ -101,7 +107,7 @@ async fn service_task( static SERVICE: StaticCell = StaticCell::new(); let service = SERVICE.init(service); - type_c_service::task::task(service, wrappers).await; + type_c_service::task::task(service, wrappers, power_policy_context).await; } #[embassy_executor::main] @@ -111,8 +117,13 @@ async fn main(spawner: Spawner) { info!("Embedded service init"); embedded_services::init().await; + static POWER_POLICY_SERVICE: StaticCell> = StaticCell::new(); + let power_service = POWER_POLICY_SERVICE.init(power_policy_service::PowerPolicy::new( + power_policy_service::Config::default(), + )); + info!("Spawining power policy task"); - spawner.must_spawn(power_policy_service_task()); + spawner.must_spawn(power_policy_service_task(power_service)); static CONTROLLER_LIST: StaticCell = StaticCell::new(); let controllers = CONTROLLER_LIST.init(IntrusiveList::new()); @@ -152,15 +163,17 @@ async fn main(spawner: Spawner) { .await .unwrap(); - static STORAGE: StaticCell> = StaticCell::new(); + static STORAGE: StaticCell> = StaticCell::new(); let storage = STORAGE.init(Storage::new( controller_context, CONTROLLER0_ID, 0, // CFU component ID [(PORT0_ID, PORT0_PWR_ID), (PORT1_ID, PORT1_PWR_ID)], + &power_service.context, )); - static REFERENCED: StaticCell> = StaticCell::new(); + static REFERENCED: StaticCell> = + StaticCell::new(); let referenced = REFERENCED.init( storage .create_referenced() @@ -176,7 +189,12 @@ async fn main(spawner: Spawner) { WRAPPER.init(ControllerWrapper::try_new(controller_mutex, Default::default(), referenced, Validator).unwrap()); info!("Spawining type-c service task"); - spawner.must_spawn(service_task(controller_context, controllers, [wrapper])); + spawner.must_spawn(service_task( + controller_context, + controllers, + [wrapper], + &power_service.context, + )); spawner.must_spawn(pd_controller_task(wrapper)); diff --git a/examples/rt685s-evk/src/bin/type_c_cfu.rs b/examples/rt685s-evk/src/bin/type_c_cfu.rs index 87cfed3a4..66612e85b 100644 --- a/examples/rt685s-evk/src/bin/type_c_cfu.rs +++ b/examples/rt685s-evk/src/bin/type_c_cfu.rs @@ -47,7 +47,7 @@ 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>, Validator, POLICY_CHANNEL_SIZE>; type Controller<'a> = tps6699x::controller::Controller>; type Interrupt<'a> = tps6699x::Interrupt<'a, GlobalRawMutex, BusDevice<'a>>; @@ -59,6 +59,8 @@ const PORT1_ID: GlobalPortId = GlobalPortId(1); const PORT0_PWR_ID: PowerId = PowerId(0); const PORT1_PWR_ID: PowerId = PowerId(1); +const POLICY_CHANNEL_SIZE: usize = 1; + #[embassy_executor::task] async fn pd_controller_task(controller: &'static Wrapper<'static>) { loop { @@ -154,10 +156,14 @@ async fn fw_update_task() { } #[embassy_executor::task] -async fn power_policy_service_task() { - power_policy_service::task::task(Default::default()) - .await - .expect("Failed to start power policy service task"); +async fn power_policy_service_task(policy: &'static power_policy_service::PowerPolicy) { + power_policy_service::task::task( + policy, + None::<[&rt685s_evk_example::DummyPowerDevice; 0]>, + None::<[&rt685s_evk_example::DummyCharger; 0]>, + ) + .await + .expect("Failed to start power policy service task"); } #[embassy_executor::task] @@ -165,6 +171,7 @@ async fn service_task( controller_context: &'static Context, controllers: &'static IntrusiveList, wrappers: [&'static Wrapper<'static>; NUM_PD_CONTROLLERS], + power_policy_context: &'static embedded_services::power::policy::policy::Context, ) -> ! { info!("Starting type-c task"); @@ -187,7 +194,7 @@ async fn service_task( static SERVICE: StaticCell = StaticCell::new(); let service = SERVICE.init(service); - type_c_service::task::task(service, wrappers).await; + type_c_service::task::task(service, wrappers, power_policy_context).await; unreachable!() } @@ -199,7 +206,13 @@ async fn main(spawner: Spawner) { embedded_services::init().await; info!("Spawining power policy task"); - spawner.must_spawn(power_policy_service_task()); + + static POWER_POLICY_SERVICE: StaticCell> = StaticCell::new(); + let power_service = POWER_POLICY_SERVICE.init(power_policy_service::PowerPolicy::new( + power_policy_service::Config::default(), + )); + + spawner.must_spawn(power_policy_service_task(power_service)); static CONTROLLER_LIST: StaticCell = StaticCell::new(); let controllers = CONTROLLER_LIST.init(IntrusiveList::new()); @@ -238,15 +251,17 @@ async fn main(spawner: Spawner) { .await .unwrap(); - static STORAGE: StaticCell> = StaticCell::new(); + static STORAGE: StaticCell> = StaticCell::new(); let storage = STORAGE.init(Storage::new( controller_context, CONTROLLER0_ID, CONTROLLER0_CFU_ID, [(PORT0_ID, PORT0_PWR_ID), (PORT1_ID, PORT1_PWR_ID)], + &power_service.context, )); - static REFERENCED: StaticCell> = StaticCell::new(); + static REFERENCED: StaticCell> = + StaticCell::new(); let referenced = REFERENCED.init( storage .create_referenced() @@ -262,7 +277,12 @@ async fn main(spawner: Spawner) { WRAPPER.init(ControllerWrapper::try_new(controller_mutex, Default::default(), referenced, Validator).unwrap()); info!("Spawning type-c service task"); - spawner.must_spawn(service_task(controller_context, controllers, [wrapper])); + spawner.must_spawn(service_task( + controller_context, + controllers, + [wrapper], + &power_service.context, + )); spawner.must_spawn(pd_controller_task(wrapper)); diff --git a/examples/rt685s-evk/src/lib.rs b/examples/rt685s-evk/src/lib.rs index b40be19ff..82fbd0ff3 100644 --- a/examples/rt685s-evk/src/lib.rs +++ b/examples/rt685s-evk/src/lib.rs @@ -18,3 +18,21 @@ static BOOT_IMAGE_VERSION: u32 = 0x01000000; #[unsafe(link_section = ".keystore")] #[used] static KEYSTORE: [u8; 2048] = [0; 2048]; + +pub struct DummyCharger(embedded_services::power::policy::charger::Device); +impl embedded_services::power::policy::charger::ChargerContainer for DummyCharger { + fn get_charger(&self) -> &embedded_services::power::policy::charger::Device { + &self.0 + } +} + +pub struct DummyPowerDevice( + embedded_services::power::policy::device::Device, +); +impl embedded_services::power::policy::device::DeviceContainer + for DummyPowerDevice +{ + fn get_power_policy_device(&self) -> &embedded_services::power::policy::device::Device { + &self.0 + } +} diff --git a/examples/std/src/bin/power_policy.rs b/examples/std/src/bin/power_policy.rs index af2ee9608..99d1c2053 100644 --- a/examples/std/src/bin/power_policy.rs +++ b/examples/std/src/bin/power_policy.rs @@ -3,7 +3,7 @@ use embassy_sync::{blocking_mutex::raw::NoopRawMutex, once_lock::OnceLock, pubsu use embassy_time::{self as _, Timer}; use embedded_services::{ broadcaster::immediate as broadcaster, - power::policy::{self, ConsumerPowerCapability, PowerCapability, device, flags}, + power::policy::{self, ConsumerPowerCapability, PowerCapability, device, flags, policy::Context}, }; use log::*; use static_cell::StaticCell; @@ -18,14 +18,17 @@ const HIGH_POWER: PowerCapability = PowerCapability { current_ma: 3000, }; +const POWER_POLICY_CHANNEL_SIZE: usize = 1; +const NUM_POWER_DEVICES: usize = 2; + struct ExampleDevice { - device: policy::device::Device, + device: policy::device::Device, } impl ExampleDevice { - fn new(id: policy::DeviceId) -> Self { + fn new(id: policy::DeviceId, context_ref: &'static Context) -> Self { Self { - device: policy::device::Device::new(id), + device: policy::device::Device::new(id, context_ref), } } @@ -56,8 +59,8 @@ impl ExampleDevice { } } -impl policy::device::DeviceContainer for ExampleDevice { - fn get_power_policy_device(&self) -> &policy::device::Device { +impl policy::device::DeviceContainer for ExampleDevice { + fn get_power_policy_device(&self) -> &policy::device::Device { &self.device } } @@ -81,23 +84,25 @@ async fn device_task1(device: &'static ExampleDevice) { } #[embassy_executor::task] -async fn run(spawner: Spawner) { +async fn run(spawner: Spawner, service: &'static power_policy_service::PowerPolicy) { embedded_services::init().await; + spawner.must_spawn(receiver_task(service)); + 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(); + let device0_mock = DEVICE0.get_or_init(|| ExampleDevice::new(policy::DeviceId(0), &service.context)); spawner.must_spawn(device_task0(device0_mock)); let device0 = device0_mock.device.try_device_action().await.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(); + let device1_mock = DEVICE1.get_or_init(|| ExampleDevice::new(policy::DeviceId(1), &service.context)); spawner.must_spawn(device_task1(device1_mock)); let device1 = device1_mock.device.try_device_action().await.unwrap(); + spawner.must_spawn(power_policy_service_task(service, [device0_mock, device1_mock])); + // Plug in device 0, should become current consumer info!("Connecting device 0"); let device0 = device0.attach().await.unwrap(); @@ -156,7 +161,7 @@ async fn run(spawner: Spawner) { Timer::after_millis(250).await; info!( "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await + service.context.compute_total_provider_power_mw().await ); info!("Device 1 attach and requesting provider"); @@ -169,7 +174,7 @@ async fn run(spawner: Spawner) { Timer::after_millis(250).await; info!( "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await + service.context.compute_total_provider_power_mw().await ); // Provider upgrade should fail because device 0 is already connected @@ -182,7 +187,7 @@ async fn run(spawner: Spawner) { Timer::after_millis(250).await; info!( "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await + service.context.compute_total_provider_power_mw().await ); // Disconnect device 0 @@ -192,7 +197,7 @@ async fn run(spawner: Spawner) { Timer::after_millis(250).await; info!( "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await + service.context.compute_total_provider_power_mw().await ); // Provider upgrade should succeed now @@ -205,12 +210,12 @@ async fn run(spawner: Spawner) { Timer::after_millis(250).await; info!( "Total provider power: {} mW", - policy::policy::compute_total_provider_power_mw().await + service.context.compute_total_provider_power_mw().await ); } #[embassy_executor::task] -async fn receiver_task() { +async fn receiver_task(service: &'static power_policy_service::PowerPolicy) { static CHANNEL: StaticCell> = StaticCell::new(); let channel = CHANNEL.init(PubSubChannel::new()); @@ -220,7 +225,7 @@ async fn receiver_task() { static RECEIVER: StaticCell> = StaticCell::new(); let receiver = RECEIVER.init(broadcaster::Receiver::new(publisher)); - policy::policy::register_message_receiver(receiver).unwrap(); + service.context.register_message_receiver(receiver).unwrap(); loop { match subscriber.next_message().await { @@ -235,8 +240,11 @@ async fn receiver_task() { } #[embassy_executor::task] -async fn power_policy_task(config: power_policy_service::config::Config) { - power_policy_service::task::task(config) +async fn power_policy_service_task( + service: &'static power_policy_service::PowerPolicy, + devices: [&'static ExampleDevice; NUM_POWER_DEVICES], +) { + power_policy_service::task::task(service, Some(devices), None::<[&std_examples::type_c::DummyCharger; 0]>) .await .expect("Failed to start power policy service task"); } @@ -246,9 +254,13 @@ fn main() { static EXECUTOR: StaticCell = StaticCell::new(); let executor = EXECUTOR.init(Executor::new()); + + static SERVICE: StaticCell> = StaticCell::new(); + let service = SERVICE.init(power_policy_service::PowerPolicy::new( + power_policy_service::config::Config::default(), + )); + executor.run(|spawner| { - spawner.must_spawn(power_policy_task(power_policy_service::config::Config::default())); - spawner.must_spawn(run(spawner)); - spawner.must_spawn(receiver_task()); + spawner.must_spawn(run(spawner, service)); }); } diff --git a/examples/std/src/bin/type_c/basic.rs b/examples/std/src/bin/type_c/basic.rs index 1f8441fe5..85ec9adc2 100644 --- a/examples/std/src/bin/type_c/basic.rs +++ b/examples/std/src/bin/type_c/basic.rs @@ -1,6 +1,7 @@ use embassy_executor::{Executor, Spawner}; use embassy_sync::once_lock::OnceLock; use embassy_time::Timer; +use embedded_services::power::policy::*; use embedded_services::type_c::{Cached, ControllerId, controller}; use embedded_services::{IntrusiveList, power}; use embedded_usb_pd::ucsi::lpm; @@ -12,6 +13,7 @@ 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); +const POWER_POLICY_CHANNEL_SIZE: usize = 1; mod test_controller { use embedded_services::type_c::controller::{ControllerStatus, PortStatus}; @@ -21,7 +23,7 @@ mod test_controller { pub struct Controller<'a> { pub controller: controller::Device<'a>, - pub power_policy: power::policy::device::Device, + pub power_policy: power::policy::device::Device, } impl controller::DeviceContainer for Controller<'_> { @@ -30,17 +32,22 @@ mod test_controller { } } - impl power::policy::device::DeviceContainer for Controller<'_> { - fn get_power_policy_device(&self) -> &power::policy::device::Device { + 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, + power_id: power::policy::DeviceId, + ports: &'a [GlobalPortId], + power_context: &'static crate::power::policy::policy::Context, + ) -> Self { Self { controller: controller::Device::new(id, ports), - power_policy: power::policy::device::Device::new(power_id), + power_policy: power::policy::device::Device::new(power_id, power_context), } } @@ -124,12 +131,16 @@ mod test_controller { } #[embassy_executor::task] -async fn controller_task(controller_list: &'static IntrusiveList) { +async fn controller_task( + controller_list: &'static IntrusiveList, + power_context: &'static policy::Context, +) { static CONTROLLER: OnceLock = OnceLock::new(); 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, POWER0_ID, &PORTS, power_context)); controller::register_controller(controller_list, controller).unwrap(); loop { @@ -141,12 +152,14 @@ async fn controller_task(controller_list: &'static IntrusiveList) { async fn task(spawner: Spawner) { embedded_services::init().await; + static POWER_CONTEXT: StaticCell> = StaticCell::new(); static CONTROLLER_LIST: StaticCell = StaticCell::new(); + let power_context = POWER_CONTEXT.init(policy::Context::new()); let controller_list = CONTROLLER_LIST.init(IntrusiveList::new()); info!("Starting controller task"); - spawner.must_spawn(controller_task(controller_list)); + spawner.must_spawn(controller_task(controller_list, power_context)); // Wait for controller to be registered Timer::after_secs(1).await; diff --git a/examples/std/src/bin/type_c/external.rs b/examples/std/src/bin/type_c/external.rs index 36a27b588..1b4cbcc83 100644 --- a/examples/std/src/bin/type_c/external.rs +++ b/examples/std/src/bin/type_c/external.rs @@ -3,6 +3,7 @@ use embassy_executor::{Executor, Spawner}; use embassy_sync::mutex::Mutex; use embassy_sync::pubsub::PubSubChannel; use embassy_time::Timer; +use embedded_services::power::policy::*; use embedded_services::{ GlobalRawMutex, IntrusiveList, power, type_c::{Cached, ControllerId, controller::Context}, @@ -18,6 +19,7 @@ const NUM_PD_CONTROLLERS: usize = 1; const CONTROLLER0_ID: ControllerId = ControllerId(0); const PORT0_ID: GlobalPortId = GlobalPortId(0); const POWER0_ID: power::policy::DeviceId = power::policy::DeviceId(0); +const POLICY_CHANNEL_SIZE: usize = 1; #[embassy_executor::task] async fn controller_task(wrapper: &'static Wrapper<'static>) { @@ -98,6 +100,7 @@ async fn service_task( controller_context: &'static Context, controllers: &'static IntrusiveList, wrappers: [&'static Wrapper<'static>; NUM_PD_CONTROLLERS], + power_context: &'static policy::Context, ) { info!("Starting type-c task"); @@ -121,22 +124,27 @@ async fn service_task( static SERVICE: StaticCell = StaticCell::new(); let service = SERVICE.init(service); - type_c_service::task::task(service, wrappers).await; + type_c_service::task::task(service, wrappers, power_context).await; } -fn create_wrapper(controller_context: &'static Context) -> &'static mut Wrapper<'static> { +fn create_wrapper( + controller_context: &'static Context, + power_context: &'static policy::Context, +) -> &'static mut Wrapper<'static> { static STATE: StaticCell = StaticCell::new(); let state = STATE.init(mock_controller::ControllerState::new()); - static STORAGE: StaticCell> = StaticCell::new(); + static STORAGE: StaticCell> = StaticCell::new(); let backing_storage = STORAGE.init(Storage::new( controller_context, CONTROLLER0_ID, 0, // CFU component ID (unused) [(PORT0_ID, POWER0_ID)], + power_context, )); - static REFERENCED: StaticCell> = - StaticCell::new(); + static REFERENCED: StaticCell< + type_c_service::wrapper::backing::ReferencedStorage<1, GlobalRawMutex, POLICY_CHANNEL_SIZE>, + > = StaticCell::new(); let referenced = REFERENCED.init( backing_storage .create_referenced() @@ -165,13 +173,15 @@ fn main() { let controller_list = CONTROLLER_LIST.init(IntrusiveList::new()); static CONTEXT: StaticCell = StaticCell::new(); let context = CONTEXT.init(embedded_services::type_c::controller::Context::new()); + static POWER_CONTEXT: StaticCell> = StaticCell::new(); + let power_context = POWER_CONTEXT.init(policy::Context::new()); - let wrapper = create_wrapper(context); + let wrapper = create_wrapper(context, power_context); static EXECUTOR: StaticCell = StaticCell::new(); let executor = EXECUTOR.init(Executor::new()); executor.run(|spawner| { - spawner.must_spawn(service_task(context, controller_list, [wrapper])); + spawner.must_spawn(service_task(context, controller_list, [wrapper], power_context)); spawner.must_spawn(task(spawner, context)); spawner.must_spawn(controller_task(wrapper)); }); diff --git a/examples/std/src/bin/type_c/service.rs b/examples/std/src/bin/type_c/service.rs index 5b7d3373a..d47b6e2d8 100644 --- a/examples/std/src/bin/type_c/service.rs +++ b/examples/std/src/bin/type_c/service.rs @@ -3,6 +3,7 @@ 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; @@ -25,6 +26,8 @@ const PORT0_ID: GlobalPortId = GlobalPortId(0); const POWER0_ID: power::policy::DeviceId = power::policy::DeviceId(0); const DELAY_MS: u64 = 1000; +const POLICY_CHANNEL_SIZE: usize = 1; + mod debug { use embedded_services::{ comms::{self, Endpoint, EndpointID, Internal}, @@ -129,10 +132,14 @@ async fn task( } #[embassy_executor::task] -async fn power_policy_service_task() { - power_policy_service::task::task(Default::default()) - .await - .expect("Failed to start power policy service task"); +async fn power_policy_service_task(policy: &'static power_policy_service::PowerPolicy) { + power_policy_service::task::task( + policy, + None::<[&std_examples::type_c::DummyPowerDevice; 0]>, + None::<[&std_examples::type_c::DummyCharger; 0]>, + ) + .await + .expect("Failed to start power policy service task"); } #[embassy_executor::task] @@ -140,6 +147,7 @@ async fn service_task( controller_context: &'static Context, controllers: &'static IntrusiveList, wrappers: [&'static Wrapper<'static>; NUM_PD_CONTROLLERS], + power_policy_context: &'static policy::Context, ) { info!("Starting type-c task"); @@ -163,11 +171,12 @@ async fn service_task( static SERVICE: StaticCell = StaticCell::new(); let service = SERVICE.init(service); - type_c_service::task::task(service, wrappers).await; + type_c_service::task::task(service, wrappers, power_policy_context).await; } fn create_wrapper( context: &'static Context, + power_policy_context: &'static policy::Context, ) -> ( &'static mut Wrapper<'static>, &'static Mutex>, @@ -176,14 +185,15 @@ fn create_wrapper( static STATE: StaticCell = StaticCell::new(); let state = STATE.init(mock_controller::ControllerState::new()); - static STORAGE: StaticCell> = StaticCell::new(); + static STORAGE: StaticCell> = StaticCell::new(); let storage = STORAGE.init(Storage::new( context, CONTROLLER0_ID, 0, // CFU component ID (unused) [(PORT0_ID, POWER0_ID)], + power_policy_context, )); - static REFERENCED: StaticCell> = StaticCell::new(); + static REFERENCED: StaticCell> = StaticCell::new(); let referenced = REFERENCED.init( storage .create_referenced() @@ -220,11 +230,21 @@ fn main() { static CONTEXT: StaticCell = StaticCell::new(); let controller_context = CONTEXT.init(embedded_services::type_c::controller::Context::new()); - let (wrapper, controller, state) = create_wrapper(controller_context); + static POWER_POLICY_SERVICE: StaticCell> = StaticCell::new(); + let power_policy_service = POWER_POLICY_SERVICE.init(power_policy_service::PowerPolicy::new( + power_policy_service::Config::default(), + )); + + let (wrapper, controller, state) = create_wrapper(controller_context, &power_policy_service.context); executor.run(|spawner| { - spawner.must_spawn(power_policy_service_task()); - spawner.must_spawn(service_task(controller_context, controller_list, [wrapper])); + spawner.must_spawn(power_policy_service_task(power_policy_service)); + spawner.must_spawn(service_task( + controller_context, + controller_list, + [wrapper], + &power_policy_service.context, + )); spawner.must_spawn(task(spawner, wrapper, controller, state)); }); } diff --git a/examples/std/src/bin/type_c/ucsi.rs b/examples/std/src/bin/type_c/ucsi.rs index bd8a617b5..1600ae2b6 100644 --- a/examples/std/src/bin/type_c/ucsi.rs +++ b/examples/std/src/bin/type_c/ucsi.rs @@ -31,6 +31,8 @@ const POWER1_ID: policy::DeviceId = policy::DeviceId(1); const CFU0_ID: u8 = 0x00; const CFU1_ID: u8 = 0x01; +const POLICY_CHANNEL_SIZE: usize = 1; + #[embassy_executor::task] async fn opm_task(context: &'static Context, state: [&'static mock_controller::ControllerState; NUM_PD_CONTROLLERS]) { const CAPABILITY: PowerCapability = PowerCapability { @@ -171,10 +173,14 @@ 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()) - .await - .expect("Failed to start power policy service task"); +async fn power_policy_service_task(policy: &'static power_policy_service::PowerPolicy) { + power_policy_service::task::task( + policy, + None::<[&std_examples::type_c::DummyPowerDevice; 0]>, + None::<[&std_examples::type_c::DummyCharger; 0]>, + ) + .await + .expect("Failed to start power policy service task"); } #[embassy_executor::task] @@ -183,6 +189,7 @@ async fn service_task( controller_context: &'static Context, controllers: &'static IntrusiveList, wrappers: [&'static Wrapper<'static>; NUM_PD_CONTROLLERS], + power_policy_context: &'static embedded_services::power::policy::policy::Context, ) -> ! { info!("Starting type-c task"); @@ -207,7 +214,7 @@ async fn service_task( static SERVICE: StaticCell = StaticCell::new(); let service = SERVICE.init(service); - type_c_service::task::task(service, wrappers).await; + type_c_service::task::task(service, wrappers, power_policy_context).await; unreachable!() } @@ -222,9 +229,20 @@ async fn type_c_service_task(spawner: Spawner) { static CONTEXT: StaticCell = StaticCell::new(); 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(); + static POWER_POLICY_SERVICE: StaticCell> = StaticCell::new(); + let power_service = POWER_POLICY_SERVICE.init(power_policy_service::PowerPolicy::new( + power_policy_service::Config::default(), + )); + + static STORAGE0: StaticCell> = StaticCell::new(); + let storage0 = STORAGE0.init(Storage::new( + context, + CONTROLLER0_ID, + CFU0_ID, + [(PORT0_ID, POWER0_ID)], + &power_service.context, + )); + static REFERENCED0: StaticCell> = StaticCell::new(); let referenced0 = REFERENCED0.init( storage0 .create_referenced() @@ -241,9 +259,15 @@ async fn type_c_service_task(spawner: Spawner) { .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(); + static STORAGE1: StaticCell> = StaticCell::new(); + let storage1 = STORAGE1.init(Storage::new( + context, + CONTROLLER1_ID, + CFU1_ID, + [(PORT1_ID, POWER1_ID)], + &power_service.context, + )); + static REFERENCED1: StaticCell> = StaticCell::new(); let referenced1 = REFERENCED1.init( storage1 .create_referenced() @@ -260,7 +284,7 @@ async fn type_c_service_task(spawner: Spawner) { .expect("Failed to create wrapper"), ); - spawner.must_spawn(power_policy_service_task()); + spawner.must_spawn(power_policy_service_task(power_service)); spawner.must_spawn(service_task( Config { ucsi_capabilities: UcsiCapabilities { @@ -289,6 +313,7 @@ async fn type_c_service_task(spawner: Spawner) { context, controller_list, [wrapper0, wrapper1], + &power_service.context, )); spawner.must_spawn(wrapper_task(wrapper0)); spawner.must_spawn(wrapper_task(wrapper1)); diff --git a/examples/std/src/bin/type_c/unconstrained.rs b/examples/std/src/bin/type_c/unconstrained.rs index c9f489b88..8b896bbb9 100644 --- a/examples/std/src/bin/type_c/unconstrained.rs +++ b/examples/std/src/bin/type_c/unconstrained.rs @@ -35,6 +35,8 @@ const CFU2_ID: u8 = 0x02; const DELAY_MS: u64 = 1000; +const POLICY_CHANNEL_SIZE: usize = 1; + #[embassy_executor::task(pool_size = 3)] async fn controller_task(wrapper: &'static mock_controller::Wrapper<'static>) { loop { @@ -98,10 +100,14 @@ 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()) - .await - .expect("Failed to start power policy service task"); +async fn power_policy_service_task(policy: &'static power_policy_service::PowerPolicy) { + power_policy_service::task::task( + policy, + None::<[&std_examples::type_c::DummyPowerDevice; 0]>, + None::<[&std_examples::type_c::DummyCharger; 0]>, + ) + .await + .expect("Failed to start power policy service task"); } #[embassy_executor::task] @@ -109,6 +115,7 @@ async fn service_task( controller_context: &'static Context, controllers: &'static IntrusiveList, wrappers: [&'static Wrapper<'static>; NUM_PD_CONTROLLERS], + power_policy_context: &'static embedded_services::power::policy::policy::Context, ) -> ! { info!("Starting type-c task"); @@ -132,7 +139,7 @@ async fn service_task( static SERVICE: StaticCell = StaticCell::new(); let service = SERVICE.init(service); - type_c_service::task::task(service, wrappers).await; + type_c_service::task::task(service, wrappers, power_policy_context).await; unreachable!() } @@ -147,9 +154,20 @@ fn main() { 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(); + static POWER_POLICY_SERVICE: StaticCell> = StaticCell::new(); + let power_service = POWER_POLICY_SERVICE.init(power_policy_service::PowerPolicy::new( + power_policy_service::Config::default(), + )); + + static STORAGE: StaticCell> = StaticCell::new(); + let storage = STORAGE.init(Storage::new( + context, + CONTROLLER0_ID, + CFU0_ID, + [(PORT0_ID, POWER0_ID)], + &power_service.context, + )); + static REFERENCED: StaticCell> = StaticCell::new(); let referenced = REFERENCED.init( storage .create_referenced() @@ -171,9 +189,15 @@ fn main() { .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(); + static STORAGE1: StaticCell> = StaticCell::new(); + let storage1 = STORAGE1.init(Storage::new( + context, + CONTROLLER1_ID, + CFU1_ID, + [(PORT1_ID, POWER1_ID)], + &power_service.context, + )); + static REFERENCED1: StaticCell> = StaticCell::new(); let referenced1 = REFERENCED1.init( storage1 .create_referenced() @@ -195,9 +219,15 @@ fn main() { .expect("Failed to create wrapper"), ); - 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(); + static STORAGE2: StaticCell> = StaticCell::new(); + let storage2 = STORAGE2.init(Storage::new( + context, + CONTROLLER2_ID, + CFU2_ID, + [(PORT2_ID, POWER2_ID)], + &power_service.context, + )); + static REFERENCED2: StaticCell> = StaticCell::new(); let referenced2 = REFERENCED2.init( storage2 .create_referenced() @@ -220,8 +250,13 @@ fn main() { ); executor.run(|spawner| { - spawner.must_spawn(power_policy_service_task()); - spawner.must_spawn(service_task(context, controller_list, [wrapper0, wrapper1, wrapper2])); + spawner.must_spawn(power_policy_service_task(power_service)); + spawner.must_spawn(service_task( + context, + controller_list, + [wrapper0, wrapper1, wrapper2], + &power_service.context, + )); spawner.must_spawn(task([state0, state1, state2])); info!("Starting controller tasks"); spawner.must_spawn(controller_task(wrapper0)); diff --git a/examples/std/src/lib/type_c/mock_controller.rs b/examples/std/src/lib/type_c/mock_controller.rs index 766373508..810362c5b 100644 --- a/examples/std/src/lib/type_c/mock_controller.rs +++ b/examples/std/src/lib/type_c/mock_controller.rs @@ -18,6 +18,8 @@ use embedded_usb_pd::{type_c::ConnectionState, ucsi::lpm}; use log::{debug, info, trace}; use std::cell::Cell; +const POWER_POLICY_CHANNEL_SIZE: usize = 1; + pub struct ControllerState { events: Signal, status: Mutex, @@ -336,5 +338,10 @@ 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>, + Validator, + POWER_POLICY_CHANNEL_SIZE, +>; diff --git a/examples/std/src/lib/type_c/mod.rs b/examples/std/src/lib/type_c/mod.rs index 4fc4ccf55..bf900859f 100644 --- a/examples/std/src/lib/type_c/mod.rs +++ b/examples/std/src/lib/type_c/mod.rs @@ -1 +1,19 @@ pub mod mock_controller; + +pub struct DummyCharger(embedded_services::power::policy::charger::Device); +impl embedded_services::power::policy::charger::ChargerContainer for DummyCharger { + fn get_charger(&self) -> &embedded_services::power::policy::charger::Device { + &self.0 + } +} + +pub struct DummyPowerDevice( + embedded_services::power::policy::device::Device, +); +impl embedded_services::power::policy::device::DeviceContainer + for DummyPowerDevice +{ + fn get_power_policy_device(&self) -> &embedded_services::power::policy::device::Device { + &self.0 + } +} diff --git a/power-policy-service/src/consumer.rs b/power-policy-service/src/consumer.rs index 0600193fc..d1162858d 100644 --- a/power-policy-service/src/consumer.rs +++ b/power-policy-service/src/consumer.rs @@ -2,8 +2,6 @@ use core::cmp::Ordering; use embedded_services::debug; use embedded_services::power::policy::charger::Device as ChargerDevice; use embedded_services::power::policy::charger::PolicyEvent; -use embedded_services::power::policy::policy::check_chargers_ready; -use embedded_services::power::policy::policy::init_chargers; use super::*; @@ -31,14 +29,14 @@ fn cmp_consumer_capability( (a.capability, a_is_current).cmp(&(b.capability, b_is_current)) } -impl PowerPolicy { +impl PowerPolicy { /// 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 +90,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; @@ -139,8 +137,8 @@ impl PowerPolicy { // Force charger CheckReady and InitRequest to get it into an initialized state. // This condition can get hit if we did not have a previous consumer and the charger is unpowered. info!("Charger is unpowered, forcing charger CheckReady and Init sequence"); - check_chargers_ready().await?; - init_chargers().await?; + self.context.check_chargers_ready().await?; + self.context.init_chargers().await?; device .execute_command(PolicyEvent::PolicyConfiguration( connected_consumer.consumer_power_capability, diff --git a/power-policy-service/src/lib.rs b/power-policy-service/src/lib.rs index c43719f81..a267ea43f 100644 --- a/power-policy-service/src/lib.rs +++ b/power-policy-service/src/lib.rs @@ -29,9 +29,9 @@ struct InternalState { } /// Power policy state -pub struct PowerPolicy { +pub struct PowerPolicy { /// Power policy context - context: policy::ContextToken, + pub context: policy::Context, /// State state: Mutex, /// Comms endpoint @@ -40,15 +40,15 @@ pub struct PowerPolicy { config: config::Config, } -impl PowerPolicy { +impl PowerPolicy { /// Create a new power policy - pub fn create(config: config::Config) -> Option { - Some(Self { - context: policy::ContextToken::create()?, + pub fn new(config: config::Config) -> Self { + Self { + context: policy::Context::new(), state: Mutex::new(InternalState::default()), tp: comms::Endpoint::uninit(comms::EndpointID::Internal(comms::Internal::Power)), config, - }) + } } async fn process_notify_attach(&self) -> Result<(), Error> { @@ -56,7 +56,7 @@ impl PowerPolicy { Ok(()) } - async fn process_notify_detach(&self, device: &device::Device) -> Result<(), Error> { + 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?; @@ -75,7 +75,7 @@ impl PowerPolicy { Ok(()) } - async fn process_notify_disconnect(&self, device: &device::Device) -> Result<(), Error> { + 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); @@ -162,4 +162,4 @@ impl PowerPolicy { } } -impl comms::MailboxDelegate for PowerPolicy {} +impl comms::MailboxDelegate for PowerPolicy {} diff --git a/power-policy-service/src/provider.rs b/power-policy-service/src/provider.rs index 830509576..0f134bcb1 100644 --- a/power-policy-service/src/provider.rs +++ b/power-policy-service/src/provider.rs @@ -25,7 +25,7 @@ pub(super) struct State { state: PowerState, } -impl PowerPolicy { +impl PowerPolicy { /// Attempt to connect the requester as a provider pub(super) async fn connect_provider(&self, requester_id: DeviceId) { trace!("Device{}: Attempting to connect as provider", requester_id.0); @@ -48,7 +48,11 @@ impl PowerPolicy { 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 diff --git a/power-policy-service/src/task.rs b/power-policy-service/src/task.rs index 916b2909f..f33327e2d 100644 --- a/power-policy-service/src/task.rs +++ b/power-policy-service/src/task.rs @@ -1,32 +1,79 @@ -use embassy_sync::once_lock::OnceLock; -use embedded_services::{comms, error, info}; +use embedded_services::{ + comms, error, info, + power::policy::{charger, device}, +}; -use crate::{PowerPolicy, config}; +use crate::PowerPolicy; #[derive(Debug, Clone, Copy)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum InitError { - /// Power policy singleton has already been initialized - AlreadyInitialized, /// Comms registration failed RegistrationFailed, + /// Power device registration failed + PowerDeviceRegistrationFailed, + /// Charger device registration failed + ChargerDeviceRegistrationFailed, } -pub async fn task(config: config::Config) -> Result { +/// Initializes and runs the power policy task. +/// +/// This task function initializes the power policy service by registering its endpoint +/// with the comms layer, registering any provided **non Type-C** power devices and charger devices, +/// and then continuously processes incoming power policy requests. It should be run as its own task +/// that never returns. +/// +/// # Generic Parameters +/// +/// * `POLICY_CHANNEL_SIZE` - The capacity of the channel used for power policy messages. +/// * `NUM_POWER_DEVICES` - The number of **non Type-C** power devices to be managed by power policy. +/// * `NUM_CHARGERS` - The number of charger devices to be managed by power policy. +/// +/// # Arguments +/// +/// * `policy` - A static reference to the [`PowerPolicy`] instance that manages power policies. +/// * `power_devices` - An optional array of static references to **non Type-C** power device containers. +/// If provided, each device will be registered with the policy context. +/// * `charger_devices` - An optional array of static references to charger device containers. +/// If provided, each charger will be registered with the policy context. +/// +/// # Returns +/// +/// Returns `Result`. The `Never` type indicates that +/// this function runs indefinitely once initialized. The function returns an error if +/// initialization fails at any stage: +/// - [`InitError::RegistrationFailed`] - if comms endpoint registration fails +/// - [`InitError::PowerDeviceRegistrationFailed`] - if power device registration fails +/// - [`InitError::ChargerDeviceRegistrationFailed`] - if charger device registration fails +pub async fn task( + policy: &'static PowerPolicy, + power_devices: Option<[&'static impl device::DeviceContainer; NUM_POWER_DEVICES]>, + charger_devices: Option<[&'static impl charger::ChargerContainer; NUM_CHARGERS]>, +) -> Result { 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"); return Err(InitError::RegistrationFailed); } + if let Some(power_devices) = power_devices { + for device in power_devices { + policy + .context + .register_device(device) + .map_err(|_| InitError::PowerDeviceRegistrationFailed)?; + } + } + + if let Some(charger_devices) = charger_devices { + for device in charger_devices { + policy + .context + .register_charger(device) + .map_err(|_| InitError::ChargerDeviceRegistrationFailed)?; + } + } + loop { if let Err(e) = policy.process().await { error!("Error processing request: {:?}", e); diff --git a/type-c-service/src/service/mod.rs b/type-c-service/src/service/mod.rs index 4637f39e1..95c48a636 100644 --- a/type-c-service/src/service/mod.rs +++ b/type-c-service/src/service/mod.rs @@ -252,8 +252,11 @@ impl<'a> Service<'a> { } /// Register the Type-C service with the power policy service - pub fn register_comms(&'static self) -> Result<(), intrusive_list::Error> { - power_policy::policy::register_message_receiver(&self.power_policy_event_publisher) + pub fn register_comms( + &'static self, + power_policy_context: &power_policy::policy::Context, + ) -> Result<(), intrusive_list::Error> { + power_policy_context.register_message_receiver(&self.power_policy_event_publisher) } pub(crate) fn controllers(&self) -> &'a intrusive_list::IntrusiveList { diff --git a/type-c-service/src/task.rs b/type-c-service/src/task.rs index 11664a1dc..b328003fb 100644 --- a/type-c-service/src/task.rs +++ b/type-c-service/src/task.rs @@ -4,9 +4,19 @@ use embedded_services::{error, info}; 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, + C, + V, + Fut: Future, + F: Fn(&'a Service) -> Fut, + const N: usize, + const POLICY_CHANNEL_SIZE: usize, +>( service: &'static Service<'a>, - wrappers: [&'a ControllerWrapper<'a, M, C, V>; N], + wrappers: [&'a ControllerWrapper<'a, M, C, V, POLICY_CHANNEL_SIZE>; N], + power_policy_context: &'a embedded_services::power::policy::policy::Context, f: F, ) where M: embassy_sync::blocking_mutex::raw::RawMutex, @@ -16,13 +26,17 @@ pub async fn task_closure<'a, M, C, V, Fut: Future, F: Fn(&'a Servi { info!("Starting type-c task"); - if service.register_comms().is_err() { + if service.register_comms(power_policy_context).is_err() { error!("Failed to register type-c service endpoint"); return; } for controller_wrapper in wrappers { - if controller_wrapper.register(service.controllers()).await.is_err() { + if controller_wrapper + .register(service.controllers(), power_policy_context) + .await + .is_err() + { error!("Failed to register a controller"); return; } @@ -34,16 +48,17 @@ 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, C, V, const N: usize, const POLICY_CHANNEL_SIZE: usize>( service: &'static Service<'a>, - wrappers: [&'a ControllerWrapper<'a, M, C, V>; N], + wrappers: [&'a ControllerWrapper<'a, M, C, V, POLICY_CHANNEL_SIZE>; N], + power_policy_context: &'a embedded_services::power::policy::policy::Context, ) where M: embassy_sync::blocking_mutex::raw::RawMutex, C: embedded_services::sync::Lockable, V: crate::wrapper::FwOfferValidator, ::Inner: embedded_services::type_c::controller::Controller, { - task_closure(service, wrappers, |service: &Service| async { + task_closure(service, wrappers, power_policy_context, |service: &Service| async { if let Err(e) = service.process_next_event().await { error!("Type-C service processing error: {:#?}", e); } diff --git a/type-c-service/src/wrapper/backing.rs b/type-c-service/src/wrapper/backing.rs index 711b5a02e..6c5d2db60 100644 --- a/type-c-service/src/wrapper/backing.rs +++ b/type-c-service/src/wrapper/backing.rs @@ -25,16 +25,21 @@ //! //! //! const NUM_PORTS: usize = 2; +//! const POLICY_CHANNEL_SIZE: usize = 1; //! -//! fn init(context: &'static embedded_services::type_c::controller::Context) { -//! static STORAGE: StaticCell> = StaticCell::new(); +//! fn init( +//! context: &'static embedded_services::type_c::controller::Context, +//! power_policy_context: &'static embedded_services::power::policy::policy::Context +//! ) { +//! 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))], +//! power_policy_context //! )); -//! static REFERENCED: StaticCell> = StaticCell::new(); +//! static REFERENCED: StaticCell> = StaticCell::new(); //! let referenced = REFERENCED.init(storage.create_referenced().unwrap()); //! let _backing = referenced.create_backing().unwrap(); //! } @@ -100,7 +105,9 @@ struct InternalState<'a, const N: usize> { } impl<'a, const N: usize> InternalState<'a, N> { - fn try_new(storage: &'a Storage) -> Option { + fn try_new( + storage: &'a Storage, + ) -> Option { let port_states = storage.pd_alerts.each_ref().map(|pd_alert| { Some(PortState { status: PortStatus::new(), @@ -158,14 +165,14 @@ pub trait DynPortState<'a> { } /// Service registration objects -pub struct Registration<'a> { +pub struct Registration<'a, const POLICY_CHANNEL_SIZE: usize> { 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], } -impl<'a> Registration<'a> { +impl<'a, const POLICY_CHANNEL_SIZE: usize> Registration<'a, POLICY_CHANNEL_SIZE> { pub fn num_ports(&self) -> usize { self.power_devices.len() } @@ -175,37 +182,39 @@ impl<'a> Registration<'a> { const MAX_BUFFERED_PD_ALERTS: usize = 4; /// Base storage -pub struct Storage<'a, const N: usize, M: RawMutex> { +pub struct Storage<'a, const N: usize, M: RawMutex, const POLICY_CHANNEL_SIZE: usize> { // Registration-related context: &'a embedded_services::type_c::controller::Context, controller_id: ControllerId, pd_ports: [GlobalPortId; N], cfu_device: embedded_services::cfu::component::CfuDevice, - power_devices: [embedded_services::power::policy::device::Device; N], + power_devices: [embedded_services::power::policy::device::Device; N], // State-related pd_alerts: [PubSubChannel; N], } -impl<'a, const N: usize, M: RawMutex> Storage<'a, N, M> { +impl<'a, const N: usize, M: RawMutex, const POLICY_CHANNEL_SIZE: usize> Storage<'a, N, M, POLICY_CHANNEL_SIZE> { pub fn new( context: &'a embedded_services::type_c::controller::Context, controller_id: ControllerId, cfu_id: ComponentId, ports: [(GlobalPortId, power::policy::DeviceId); N], + power_policy_context: &'static embedded_services::power::policy::policy::Context, ) -> Self { Self { context, controller_id, pd_ports: ports.map(|(port, _)| port), cfu_device: embedded_services::cfu::component::CfuDevice::new(cfu_id), - power_devices: ports.map(|(_, device)| embedded_services::power::policy::device::Device::new(device)), + power_devices: ports + .map(|(_, device)| embedded_services::power::policy::device::Device::new(device, power_policy_context)), pd_alerts: [const { PubSubChannel::new() }; N], } } /// Create referenced storage from this storage - pub fn create_referenced(&self) -> Option> { + pub fn create_referenced(&self) -> Option> { ReferencedStorage::try_from_storage(self) } } @@ -214,15 +223,17 @@ 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>, +pub struct ReferencedStorage<'a, const N: usize, M: RawMutex, const POLICY_CHANNEL_SIZE: usize> { + storage: &'a Storage<'a, N, M, POLICY_CHANNEL_SIZE>, state: RefCell>, pd_controller: embedded_services::type_c::controller::Device<'a>, } -impl<'a, const N: usize, M: RawMutex> ReferencedStorage<'a, N, M> { +impl<'a, const N: usize, M: RawMutex, const POLICY_CHANNEL_SIZE: usize> + ReferencedStorage<'a, N, M, POLICY_CHANNEL_SIZE> +{ /// Create a new referenced storage from the given storage and controller ID - fn try_from_storage(storage: &'a Storage) -> Option { + fn try_from_storage(storage: &'a Storage) -> Option { Some(Self { storage, state: RefCell::new(InternalState::try_new(storage)?), @@ -234,7 +245,7 @@ impl<'a, const N: usize, M: RawMutex> ReferencedStorage<'a, N, M> { } /// 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, { @@ -251,7 +262,7 @@ impl<'a, const N: usize, M: RawMutex> ReferencedStorage<'a, N, M> { } /// Wrapper around registration and type-erased state -pub struct Backing<'a> { - pub(crate) registration: Registration<'a>, +pub struct Backing<'a, const POLICY_CHANNEL_SIZE: usize> { + pub(crate) registration: Registration<'a, POLICY_CHANNEL_SIZE>, pub(crate) state: RefMut<'a, dyn DynPortState<'a>>, } diff --git a/type-c-service/src/wrapper/cfu.rs b/type-c-service/src/wrapper/cfu.rs index d1b7f7246..209ffbe25 100644 --- a/type-c-service/src/wrapper/cfu.rs +++ b/type-c-service/src/wrapper/cfu.rs @@ -29,7 +29,8 @@ impl FwUpdateState { } } -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator, const POLICY_CHANNEL_SIZE: usize> + ControllerWrapper<'device, M, C, V, POLICY_CHANNEL_SIZE> where ::Inner: Controller, { diff --git a/type-c-service/src/wrapper/dp.rs b/type-c-service/src/wrapper/dp.rs index 8bae8560f..1b9882c8e 100644 --- a/type-c-service/src/wrapper/dp.rs +++ b/type-c-service/src/wrapper/dp.rs @@ -4,7 +4,8 @@ use embassy_sync::blocking_mutex::raw::RawMutex; use embedded_services::{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, C: Lockable, V: FwOfferValidator, const POLICY_CHANNEL_SIZE: usize> + ControllerWrapper<'device, M, C, V, POLICY_CHANNEL_SIZE> where ::Inner: Controller, { diff --git a/type-c-service/src/wrapper/mod.rs b/type-c-service/src/wrapper/mod.rs index fb2853c0c..9cb42a8d6 100644 --- a/type-c-service/src/wrapper/mod.rs +++ b/type-c-service/src/wrapper/mod.rs @@ -68,7 +68,7 @@ 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> +pub struct ControllerWrapper<'device, M: RawMutex, C: Lockable, V: FwOfferValidator, const POLICY_CHANNEL_SIZE: usize> where ::Inner: Controller, { @@ -78,7 +78,7 @@ where /// 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, POLICY_CHANNEL_SIZE>, /// State state: Mutex>>, /// SW port status event signal @@ -87,7 +87,8 @@ where config: config::Config, } -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator, const POLICY_CHANNEL_SIZE: usize> + ControllerWrapper<'device, M, C, V, POLICY_CHANNEL_SIZE> where ::Inner: Controller, { @@ -95,7 +96,7 @@ where pub fn try_new( controller: &'device C, config: config::Config, - storage: &'device backing::ReferencedStorage<'device, N, M>, + storage: &'device backing::ReferencedStorage<'device, N, M, POLICY_CHANNEL_SIZE>, fw_version_validator: V, ) -> Option { const { @@ -117,7 +118,7 @@ where } /// Get the power policy devices for this controller. - pub fn power_policy_devices(&self) -> &[policy::device::Device] { + pub fn power_policy_devices(&self) -> &[policy::device::Device] { self.registration.power_devices } @@ -180,7 +181,7 @@ where async fn process_plug_event( &self, _controller: &mut C::Inner, - power: &policy::device::Device, + power: &policy::device::Device, port: LocalPortId, status: &PortStatus, ) -> Result<(), Error<::BusError>> { @@ -610,10 +611,11 @@ where pub async fn register( &'static self, controllers: &intrusive_list::IntrusiveList, + power_policy_context: &embedded_services::power::policy::policy::Context, ) -> Result<(), Error<::BusError>> { // TODO: Unify these devices? for device in self.registration.power_devices { - policy::register_device(device).map_err(|_| { + power_policy_context.register_device(device).map_err(|_| { error!( "Controller{}: Failed to register power device {}", self.registration.pd_controller.id().0, @@ -645,7 +647,8 @@ where } } -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> Lockable for ControllerWrapper<'device, M, C, V> +impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator, const POLICY_CHANNEL_SIZE: usize> Lockable + for ControllerWrapper<'device, M, C, V, POLICY_CHANNEL_SIZE> where ::Inner: Controller, { diff --git a/type-c-service/src/wrapper/pd.rs b/type-c-service/src/wrapper/pd.rs index f55bd8bbf..4d1660618 100644 --- a/type-c-service/src/wrapper/pd.rs +++ b/type-c-service/src/wrapper/pd.rs @@ -9,7 +9,8 @@ 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, C: Lockable, V: FwOfferValidator, const POLICY_CHANNEL_SIZE: usize> + ControllerWrapper<'device, M, C, V, POLICY_CHANNEL_SIZE> where ::Inner: Controller, { diff --git a/type-c-service/src/wrapper/power.rs b/type-c-service/src/wrapper/power.rs index 2d3682a1d..f2e6a5b29 100644 --- a/type-c-service/src/wrapper/power.rs +++ b/type-c-service/src/wrapper/power.rs @@ -15,19 +15,20 @@ use crate::wrapper::config::UnconstrainedSink; use super::*; -impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator> ControllerWrapper<'device, M, C, V> +impl<'device, M: RawMutex, C: Lockable, V: FwOfferValidator, const POLICY_CHANNEL_SIZE: usize> + ControllerWrapper<'device, M, C, V, POLICY_CHANNEL_SIZE> where ::Inner: Controller, { /// Return the power device for the given port - pub fn get_power_device(&self, port: LocalPortId) -> Option<&policy::device::Device> { + 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: &policy::device::Device, status: &PortStatus, ) -> Result<(), Error<::BusError>> { info!("Process new consumer contract"); @@ -84,7 +85,7 @@ where /// Handle a new contract as provider pub(super) async fn process_new_provider_contract( &self, - power: &policy::device::Device, + power: &policy::device::Device, status: &PortStatus, ) -> Result<(), Error<::BusError>> { info!("Process New provider contract"); @@ -149,7 +150,7 @@ where &self, port: LocalPortId, controller: &mut C::Inner, - power: &policy::device::Device, + power: &policy::device::Device, ) -> Result<(), Error<::BusError>> { let state = power.state().await.kind(); if state == StateKind::ConnectedConsumer { diff --git a/type-c-service/src/wrapper/vdm.rs b/type-c-service/src/wrapper/vdm.rs index 029b7f7d1..c4d0ad947 100644 --- a/type-c-service/src/wrapper/vdm.rs +++ b/type-c-service/src/wrapper/vdm.rs @@ -13,7 +13,8 @@ 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, C: Lockable, V: FwOfferValidator, const POLICY_CHANNEL_SIZE: usize> + ControllerWrapper<'device, M, C, V, POLICY_CHANNEL_SIZE> where ::Inner: Controller, {