From 546b65baf9fa1d1eeb0ff046efc15fc881791961 Mon Sep 17 00:00:00 2001
From: Greg V <greg@unrelenting.technology>
Date: Thu, 21 Jan 2021 20:19:20 +0300
Subject: [PATCH] Register a connection (if supported) when opening the device

At least the J-Link clone I have (serial 123456, HW 7.00, FW
"J-Link ARM-OB STM32 compiled Aug 22 2012 19:52:04") absolutely requires
a registered connection, at least for SWD usage. Nothing ever gets ACKed otherwise.

OpenOCD always calls jaylink_register (with all zeros for info)
when the REGISTER capability is present.
---
 src/lib.rs | 48 +++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 47 insertions(+), 1 deletion(-)

diff --git a/src/lib.rs b/src/lib.rs
index 116236c..6c666e7 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -90,7 +90,7 @@ pub use self::interface::{Interface, InterfaceIter, Interfaces};
 use self::bits::IteratorExt as _;
 use self::error::ResultExt as _;
 use bitflags::bitflags;
-use byteorder::{LittleEndian, ReadBytesExt};
+use byteorder::{BigEndian, LittleEndian, ReadBytesExt, WriteBytesExt};
 use io::Cursor;
 use log::{debug, trace, warn};
 use std::cell::{Cell, RefCell, RefMut};
@@ -161,6 +161,15 @@ enum Command {
 
     ReadConfig = 0xF2,
     WriteConfig = 0xF3,
+
+    Register = 0x09,
+}
+
+#[repr(u8)]
+#[allow(dead_code)]
+enum RegisterCommand {
+    Register = 0x64,
+    Unregister = 0x65,
 }
 
 #[repr(u8)]
@@ -207,6 +216,15 @@ impl SwoStatus {
     }
 }
 
+#[derive(Default, Debug)]
+pub struct Connection {
+    handle: u16,
+    pid: u32,
+    hid: Option<std::net::Ipv4Addr>,
+    iid: u8,
+    cid: u8,
+}
+
 /// A handle to a J-Link USB device.
 ///
 /// This is the main interface type of this library. There are multiple ways of obtaining an
@@ -436,6 +454,10 @@ impl JayLink {
         this.fill_capabilities()?;
         this.fill_interfaces()?;
 
+        if this.capabilities().contains(Capabilities::REGISTER) {
+            this.register(Connection::default())?;
+        }
+
         // Probes remember the selected interface, so provide consistent defaults to avoid
         // unreliable apps.
         this.select_interface(Interface::Jtag)?;
@@ -899,6 +921,30 @@ impl JayLink {
         Ok(())
     }
 
+    /// Registers a connection on the device.
+    ///
+    /// This requires the [`REGISTER`] capability.
+    ///
+    /// **Note**: This may be **required** on some devices for SWD to work at all.
+    ///
+    /// [`REGISTER`]: struct.Capabilities.html#associatedconstant.REGISTER
+    pub fn register(&self, conn: Connection) -> Result<()> {
+        self.require_capabilities(Capabilities::REGISTER)?;
+        let mut buf = Vec::with_capacity(14);
+        buf.push(Command::Register as u8);
+        buf.push(RegisterCommand::Register as u8);
+        buf.write_u32::<LittleEndian>(conn.pid).unwrap();
+        buf.write_u32::<BigEndian>(conn.hid.unwrap_or(std::net::Ipv4Addr::LOCALHOST).into())
+            .unwrap();
+        buf.push(conn.iid);
+        buf.push(conn.cid);
+        buf.write_u16::<LittleEndian>(conn.handle).unwrap();
+        self.write_cmd(&buf)?;
+        let mut buf = [0; 76];
+        self.read(&mut buf)?;
+        Ok(())
+    }
+
     /// Performs a JTAG I/O operation.
     ///
     /// This will shift out data on `TMS` (pin 7) and `TDI` (pin 5), while reading data shifted