From 2fa00bcc36275585588969b2f76dd743cd586e56 Mon Sep 17 00:00:00 2001
From: Rahix <rahix@rahix.de>
Date: Fri, 15 Nov 2024 01:25:28 +0100
Subject: [PATCH] dp: peripheral: Add method to change the peripheral address

For applications with dynamic peripheral addresses, add a method to
conveniently reset a peripheral to a new address without reconstructing
it explicitly.
---
 src/dp/diagnostics.rs |  5 +++++
 src/dp/peripheral.rs  | 13 +++++++++++++
 2 files changed, 18 insertions(+)

diff --git a/src/dp/diagnostics.rs b/src/dp/diagnostics.rs
index bc45985..0410420 100644
--- a/src/dp/diagnostics.rs
+++ b/src/dp/diagnostics.rs
@@ -46,6 +46,11 @@ impl<'a> ExtendedDiagnostics<'a> {
         Self { buffer, length: 0 }
     }
 
+    pub(crate) fn take_buffer(&mut self) -> &'a mut [u8] {
+        self.length = 0;
+        core::mem::take(&mut self.buffer)
+    }
+
     pub(crate) fn fill(&mut self, buf: &[u8]) -> bool {
         if self.buffer.len() == 0 {
             // No buffer for ext. diagnostics so we ignore them entirely.
diff --git a/src/dp/peripheral.rs b/src/dp/peripheral.rs
index 18912ee..e7c3368 100644
--- a/src/dp/peripheral.rs
+++ b/src/dp/peripheral.rs
@@ -229,6 +229,19 @@ impl<'a> Peripheral<'a> {
         self
     }
 
+    /// Completely reset this peripheral to a new address.
+    ///
+    /// The process images are not changed by this operation.  A new DP parameterization will take
+    /// place once the device responds at the new address.
+    pub fn reset_address(&mut self, new_address: crate::Address) {
+        let options = core::mem::take(&mut self.options);
+        let pi_i = core::mem::take(&mut self.pi_i);
+        let pi_q = core::mem::take(&mut self.pi_q);
+        let diag_buffer = self.ext_diag.take_buffer();
+
+        *self = Self::new(new_address, options, pi_i, pi_q).with_diag_buffer(diag_buffer);
+    }
+
     /// Address of this peripheral.
     #[inline(always)]
     pub fn address(&self) -> u8 {