Merge pull request #291 from RobertZ2011:pd-controller-init

type-c: Add function to sync controller state

Author: jerrysxie

embedded-service/src/type_c/controller.rs

@@ -219,6 +219,11 @@ impl<'a> Device<'a> {
         }
     }
 
+    /// Get the controller ID
+    pub fn id(&self) -> ControllerId {
+        self.id
+    }
+
     /// Send a command to this controller
     pub async fn send_command(&self, command: Command) -> Response {
         self.command.send(command).await;
@@ -302,6 +307,8 @@ pub trait Controller {
     /// Type of error returned by the bus
     type BusError;
 
+    /// Ensure software state is in sync with hardware state
+    fn sync_state(&mut self) -> impl Future<Output = Result<(), Error<Self::BusError>>>;
     /// Returns ports with pending events
     fn wait_port_event(&mut self) -> impl Future<Output = Result<(), Error<Self::BusError>>>;
     /// Returns and clears current events for the given port

examples/std/src/bin/type_c/service.rs

@@ -96,6 +96,10 @@ mod test_controller {
     impl embedded_services::type_c::controller::Controller for Controller<'_> {
         type BusError = ();
 
+        async fn sync_state(&mut self) -> Result<(), Error<Self::BusError>> {
+            Ok(())
+        }
+
         async fn wait_port_event(&mut self) -> Result<(), Error<Self::BusError>> {
             trace!("Wait for port event");
             let events = self.state.events.wait().await;

type-c-service/src/driver/tps6699x.rs

@@ -46,8 +46,8 @@ impl<'a, const N: usize, M: RawMutex, B: I2c> Tps6699x<'a, N, M, B> {
         tps6699x: &mut tps6699x::Tps6699x<'a, M, B>,
         port: LocalPortId,
     ) -> Result<PortEventKind, Error<B::Error>> {
-        #[allow(unused_mut)]
         let mut events = PortEventKind::none();
+        let previous_status = self.port_status[port.0 as usize].get();
 
         let status = tps6699x.get_port_status(port).await?;
         trace!("Port{} status: {:#?}", port.0, status);
@@ -95,13 +95,6 @@ impl<'a, const N: usize, M: RawMutex, B: I2c> Tps6699x<'a, N, M, B> {
                 let current = TypecCurrent::try_from(port_control.typec_current()).map_err(Error::Pd)?;
                 debug!("Port{} type-C source current: {:#?}", port.0, current);
                 let new_contract = Some(PowerCapability::from(current));
-
-                if new_contract != port_status.available_source_contract {
-                    debug!("New implicit contract as provider");
-                    // We don't get interrupts for implicit contracts so generate event manually
-                    events.set_new_power_contract_as_provider(true);
-                }
-
                 port_status.available_source_contract = new_contract;
             } else {
                 // Implicit sink contract
@@ -115,13 +108,6 @@ impl<'a, const N: usize, M: RawMutex, B: I2c> Tps6699x<'a, N, M, B> {
                     debug!("Port{} type-C sink current: {:#?}", port.0, current);
                     Some(PowerCapability::from(current))
                 };
-
-                if new_contract.is_some() && new_contract != port_status.available_sink_contract {
-                    debug!("New implicit contract as consumer");
-                    // We don't get interrupts for implicit contracts so generate event manually
-                    events.set_new_power_contract_as_consumer(true);
-                }
-
                 port_status.available_sink_contract = new_contract;
             }
 
@@ -146,6 +132,20 @@ impl<'a, const N: usize, M: RawMutex, B: I2c> Tps6699x<'a, N, M, B> {
             debug!("Port{} power path: {:#?}", port.0, port_status.power_path);
         }
 
+        if port_status.available_sink_contract.is_some()
+            && port_status.available_sink_contract != previous_status.available_sink_contract
+        {
+            debug!("Port{}: new sink contract", port.0);
+            events.set_new_power_contract_as_consumer(true);
+        }
+
+        if port_status.available_source_contract.is_some()
+            && port_status.available_source_contract != previous_status.available_source_contract
+        {
+            debug!("Port{}: new source contract", port.0);
+            events.set_new_power_contract_as_provider(true);
+        }
+
         self.port_status[port.0 as usize].set(port_status);
         Ok(events)
     }
@@ -186,7 +186,6 @@ impl<'a, const N: usize, M: RawMutex, B: I2c> Tps6699x<'a, N, M, B> {
     }
 
     /// Signal an event on the given port
-    #[allow(dead_code)]
     fn signal_event(&self, port: LocalPortId, event: PortEventKind) {
         if port.0 >= self.port_events.len() as u8 {
             return;
@@ -202,6 +201,19 @@ impl<'a, const N: usize, M: RawMutex, B: I2c> Tps6699x<'a, N, M, B> {
 impl<const N: usize, M: RawMutex, B: I2c> Controller for Tps6699x<'_, N, M, B> {
     type BusError = B::Error;
 
+    /// Controller specific initialization
+    #[allow(clippy::await_holding_refcell_ref)]
+    async fn sync_state(&mut self) -> Result<(), Error<Self::BusError>> {
+        for i in 0..N {
+            let port = LocalPortId(i as u8);
+            let mut tps6699x = self.tps6699x.borrow_mut();
+            let event = self.update_port_status(&mut tps6699x, port).await?;
+            self.signal_event(port, event);
+        }
+
+        Ok(())
+    }
+
     /// Wait for an event on any port
     #[allow(clippy::await_holding_refcell_ref)]
     async fn wait_port_event(&mut self) -> Result<(), Error<Self::BusError>> {

type-c-service/src/wrapper/mod.rs

@@ -8,7 +8,7 @@ use embedded_services::power::policy::device::StateKind;
 use embedded_services::power::policy::{self, action};
 use embedded_services::type_c::controller::{self, Controller, PortStatus};
 use embedded_services::type_c::event::{PortEventFlags, PortEventKind};
-use embedded_services::{error, info, intrusive_list, trace, warn};
+use embedded_services::{error, info, trace, warn};
 use embedded_usb_pd::{type_c::Current as TypecCurrent, Error, PdError, PortId as LocalPortId};
 
 mod pd;
@@ -42,6 +42,13 @@ impl<'a, const N: usize, C: Controller> ControllerWrapper<'a, N, C> {
         }
     }
 
+    /// Ensure the software state is in sync with the hardware state
+    #[allow(clippy::await_holding_refcell_ref)]
+    async fn sync_state(&self) -> Result<(), Error<C::BusError>> {
+        let mut controller = self.controller.borrow_mut();
+        controller.sync_state().await
+    }
+
     /// Handle a plug event
     async fn process_plug_event(
         &self,
@@ -211,11 +218,28 @@ impl<'a, const N: usize, C: Controller> ControllerWrapper<'a, N, C> {
     }
 
     /// Register all devices with their respective services
-    pub async fn register(&'static self) -> Result<(), intrusive_list::Error> {
+    pub async fn register(&'static self) -> Result<(), Error<C::BusError>> {
         for device in &self.power {
-            policy::register_device(device).await?
+            policy::register_device(device).await.map_err(|_| {
+                error!(
+                    "Controller{}: Failed to register power device {}",
+                    self.pd_controller.id().0,
+                    device.id().0
+                );
+                Error::Pd(PdError::Failed)
+            })?;
         }
 
-        controller::register_controller(&self.pd_controller).await
+        controller::register_controller(&self.pd_controller)
+            .await
+            .map_err(|_| {
+                error!(
+                    "Controller{}: Failed to register PD controller",
+                    self.pd_controller.id().0
+                );
+                Error::Pd(PdError::Failed)
+            })?;
+
+        self.sync_state().await
     }
 }