From 31df059df859e99ead765b34c78537636746939c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Filip=20Jagodzi=C5=84ski?= <fkjagodzinski@gmail.com>
Date: Tue, 29 Nov 2022 16:50:18 +0100
Subject: [PATCH] probe: stlink: Fix 1-byte transfers (#1476)

Internally the 1-byte transfers are handled in 3 phases:
1. read/write 8-bit chunks until the first aligned address is reached,
2. read/write 32-bit chunks from all aligned addresses,
3. read/write 8-bit chunks from the remaining unaligned addresses.

Size of the first unaligned read/write is set to the result of address
alignment check (4-byte) and can be either 1, 2, or 3 bytes (the value
of `unaligned_count` calculated as `addr & 0x3`). This is incorrect and
every transfer with the requested size smaller than `unaligned_count` is
terminated with the following error:

  Unhandled exception in handle_message (b'm'): result size (3) != requested size (1) [gdbserver]

Skip the first unaligned transfer if the requested size is so small that
phase-1 would not even reach aligned address. Handle the whole request
in the second unaligned read/write (phase-3).
---
 pyocd/probe/stlink_probe.py | 23 +++++++++++++++--------
 1 file changed, 15 insertions(+), 8 deletions(-)

diff --git a/pyocd/probe/stlink_probe.py b/pyocd/probe/stlink_probe.py
index ada044b2b..877a75390 100644
--- a/pyocd/probe/stlink_probe.py
+++ b/pyocd/probe/stlink_probe.py
@@ -1,5 +1,5 @@
 # pyOCD debugger
-# Copyright (c) 2018-2020 Arm Limited
+# Copyright (c) 2018-2020,2022 Arm Limited
 # Copyright (c) 2021-2022 Chris Reed
 # SPDX-License-Identifier: Apache-2.0
 #
@@ -305,21 +305,28 @@ def read_memory_block8(self, addr: int, size: int, **attrs: Any) -> Sequence[int
         csw = attrs.get('csw', 0)
         res = []
 
-        # read leading unaligned bytes
-        unaligned_count = addr & 3
-        if (size > 0) and (unaligned_count > 0):
+        # Transfers are handled in 3 phases:
+        #   1. read 8-bit chunks until the first aligned address is reached,
+        #   2. read 32-bit chunks from all aligned addresses,
+        #   3. read 8-bit chunks from the remaining unaligned addresses.
+        # If the requested size is so small that phase-1 would not even reach
+        # aligned address, go straight to phase-3.
+
+        # 1. read leading unaligned bytes
+        unaligned_count = 3 & (4 - addr)
+        if (size > unaligned_count > 0):
             res += self._link.read_mem8(addr, unaligned_count, self._apsel, csw)
             size -= unaligned_count
             addr += unaligned_count
 
-        # read aligned block of 32 bits
+        # 2. read aligned block of 32 bits
         if (size >= 4):
             aligned_size = size & ~3
             res += self._link.read_mem32(addr, aligned_size, self._apsel, csw)
             size -= aligned_size
             addr += aligned_size
 
-        # read trailing unaligned bytes
+        # 3. read trailing unaligned bytes
         if (size > 0):
             res += self._link.read_mem8(addr, size, self._apsel, csw)
 
@@ -332,8 +339,8 @@ def write_memory_block8(self, addr: int, data: Sequence[int], **attrs: Any) -> N
         idx = 0
 
         # write leading unaligned bytes
-        unaligned_count = addr & 3
-        if (size > 0) and (unaligned_count > 0):
+        unaligned_count = 3 & (4 - addr)
+        if (size > unaligned_count > 0):
             self._link.write_mem8(addr, data[:unaligned_count], self._apsel, csw)
             size -= unaligned_count
             addr += unaligned_count