[GDB] Atomic read/write for device memory

src/core/gdbstub.c

 #include <stddef.h>
 #include <stdio.h>
 #include <ctype.h>
-#include <assert.h>
+#include <byteswap.h>
 #include <gpxe/process.h>
 #include <gpxe/serial.h>
 #include "gdbmach.h"
 	return ( b < 0xa ? '0' : 'a' - 0xa ) + b;
 }
 
-static void gdbstub_from_hex_buf ( char *dst, char *src, int len ) {
-	while ( len-- > 0 ) {
-		*dst = gdbstub_from_hex_digit ( *src++ );
-		if ( len-- > 0 ) {
-			*dst = (*dst << 4) | gdbstub_from_hex_digit ( *src++ );
+/*
+ * To make reading/writing device memory atomic, we check for
+ * 2- or 4-byte aligned operations and handle them specially.
+ */
+
+static void gdbstub_from_hex_buf ( char *dst, char *src, int lenbytes ) {
+	if ( lenbytes == 2 && ( ( unsigned long ) dst & 0x1 ) == 0 ) {
+		uint16_t i = gdbstub_from_hex_digit ( src [ 2 ] ) << 12 |
+			gdbstub_from_hex_digit ( src [ 3 ] ) << 8 |
+			gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
+			gdbstub_from_hex_digit ( src [ 1 ] );
+		* ( uint16_t * ) dst = cpu_to_le16 ( i );
+	} else if ( lenbytes == 4 && ( ( unsigned long ) dst & 0x3 ) == 0 ) {
+		uint32_t i = gdbstub_from_hex_digit ( src [ 6 ] ) << 28 |
+			gdbstub_from_hex_digit ( src [ 7 ] ) << 24 |
+			gdbstub_from_hex_digit ( src [ 4 ] ) << 20 |
+			gdbstub_from_hex_digit ( src [ 5 ] ) << 16 |
+			gdbstub_from_hex_digit ( src [ 2 ] ) << 12 |
+			gdbstub_from_hex_digit ( src [ 3 ] ) << 8 |
+			gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
+			gdbstub_from_hex_digit ( src [ 1 ] );
+		* ( uint32_t * ) dst = cpu_to_le32 ( i );
+	} else {
+		while ( lenbytes-- > 0 ) {
+			*dst++ = gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
+				gdbstub_from_hex_digit ( src [ 1 ] );
+			src += 2;
 		}
-		dst++;
 	}
 }
 
-static void gdbstub_to_hex_buf ( char *dst, char *src, int len ) {
-	while ( len-- > 0 ) {
-		*dst++ = gdbstub_to_hex_digit ( *src >> 4 );
-		*dst++ = gdbstub_to_hex_digit ( *src++ );
+static void gdbstub_to_hex_buf ( char *dst, char *src, int lenbytes ) {
+	if ( lenbytes == 2 && ( ( unsigned long ) src & 0x1 ) == 0 ) {
+		uint16_t i = cpu_to_le16 ( * ( uint16_t * ) src );
+		dst [ 0 ] = gdbstub_to_hex_digit ( i >> 4 );
+		dst [ 1 ] = gdbstub_to_hex_digit ( i );
+		dst [ 2 ] = gdbstub_to_hex_digit ( i >> 12 );
+		dst [ 3 ] = gdbstub_to_hex_digit ( i >> 8 );
+	} else if ( lenbytes == 4 && ( ( unsigned long ) src & 0x3 ) == 0 ) {
+		uint32_t i = cpu_to_le32 ( * ( uint32_t * ) src );
+		dst [ 0 ] = gdbstub_to_hex_digit ( i >> 4 );
+		dst [ 1 ] = gdbstub_to_hex_digit ( i );
+		dst [ 2 ] = gdbstub_to_hex_digit ( i >> 12 );
+		dst [ 3 ] = gdbstub_to_hex_digit ( i >> 8 );
+		dst [ 4 ] = gdbstub_to_hex_digit ( i >> 20 );
+		dst [ 5 ] = gdbstub_to_hex_digit ( i >> 16);
+		dst [ 6 ] = gdbstub_to_hex_digit ( i >> 28 );
+		dst [ 7 ] = gdbstub_to_hex_digit ( i >> 24 );
+	} else {
+		while ( lenbytes-- > 0 ) {
+			*dst++ = gdbstub_to_hex_digit ( *src >> 4 );
+			*dst++ = gdbstub_to_hex_digit ( *src );
+			src++;
+		}
 	}
 }
 
 		gdbstub_send_errno ( stub, POSIX_EINVAL );
 		return;
 	}
-	gdbstub_from_hex_buf ( ( char * ) stub->regs, &stub->payload [ 1 ], stub->len );
+	gdbstub_from_hex_buf ( ( char * ) stub->regs, &stub->payload [ 1 ], GDBMACH_SIZEOF_REGS );
 	gdbstub_send_ok ( stub );
 }
 
 		gdbstub_send_errno ( stub, POSIX_EINVAL );
 		return;
 	}
-	gdbstub_from_hex_buf ( ( char * ) args [ 0 ], &stub->payload [ colon + 1 ], stub->len - colon - 1 );
+	gdbstub_from_hex_buf ( ( char * ) args [ 0 ], &stub->payload [ colon + 1 ], ( stub->len - colon - 1 ) / 2 );
 	gdbstub_send_ok ( stub );
 }