diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 8bfe3d5a77..c3870b75d5 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -529,68 +529,70 @@ int rtos_get_gdb_reg(struct connection *connection, int reg_num)
 {
 	struct target *target = get_target_from_connection(connection);
 	threadid_t current_threadid = target->rtos->current_threadid;
-	if ((target->rtos) && (current_threadid != -1) &&
-			(current_threadid != 0) &&
-			((current_threadid != target->rtos->current_thread) ||
-			(target->smp))) {	/* in smp several current thread are possible */
-		struct rtos_reg *reg_list;
-		int num_regs;
+	if (!target->rtos ||
+			current_threadid == -1 ||
+			current_threadid == 0 ||
+			(current_threadid == target->rtos->current_thread &&
+			 !target->smp)) { /* in smp several current thread are possible */
+		return ERROR_NOT_IMPLEMENTED;
+	}
 
-		LOG_DEBUG("getting register %d for thread 0x%" PRIx64
-				  ", target->rtos->current_thread=0x%" PRIx64,
-										reg_num,
-										current_threadid,
-										target->rtos->current_thread);
-
-		int retval;
-		if (target->rtos->type->get_thread_reg_value) {
-			uint32_t reg_size;
-			uint8_t *reg_value;
-			retval = target->rtos->type->get_thread_reg_value(target->rtos,
-					current_threadid, reg_num, &reg_size, &reg_value);
-			if (retval != ERROR_OK) {
-				LOG_ERROR("RTOS: failed to get register %d", reg_num);
-				return retval;
-			}
+	struct rtos_reg *reg_list;
+	int num_regs;
 
-			/* Create a reg_list with one register that can
-			 * accommodate the full size of the one we just got the
-			 * value for. To do that we allocate extra space off the
-			 * end of the struct, relying on the fact that
-			 * rtos_reg.value is the last element in the struct. */
-			reg_list = calloc(1, sizeof(*reg_list) + DIV_ROUND_UP(reg_size, 8));
-			if (!reg_list) {
-				free(reg_value);
-				LOG_ERROR("Failed to allocated reg_list for %d-byte register.",
-					  reg_size);
-				return ERROR_FAIL;
-			}
-			reg_list[0].number = reg_num;
-			reg_list[0].size = reg_size;
-			memcpy(&reg_list[0].value, reg_value, DIV_ROUND_UP(reg_size, 8));
-			free(reg_value);
-			num_regs = 1;
-		} else {
-			retval = target->rtos->type->get_thread_reg_list(target->rtos,
-					current_threadid,
-					&reg_list,
-					&num_regs);
-			if (retval != ERROR_OK) {
-				LOG_ERROR("RTOS: failed to get register list");
-				return retval;
-			}
+	LOG_TARGET_DEBUG(target, "getting register %d for thread 0x%" PRIx64
+				", target->rtos->current_thread=0x%" PRIx64,
+				reg_num, current_threadid, target->rtos->current_thread);
+
+	int retval;
+	if (target->rtos->type->get_thread_reg_value) {
+		uint32_t reg_size;
+		uint8_t *reg_value;
+		retval = target->rtos->type->get_thread_reg_value(target->rtos,
+				current_threadid, reg_num, &reg_size, &reg_value);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("RTOS: failed to get register %d", reg_num);
+			return retval;
 		}
 
-		for (int i = 0; i < num_regs; ++i) {
-			if (reg_list[i].number == (uint32_t)reg_num) {
-				rtos_put_gdb_reg_list(connection, reg_list + i, 1);
-				free(reg_list);
-				return ERROR_OK;
-			}
+		/* Create a reg_list with one register that can
+		 * accommodate the full size of the one we just got the
+		 * value for. To do that we allocate extra space off the
+		 * end of the struct, relying on the fact that
+		 * rtos_reg.value is the last element in the struct. */
+		reg_list = calloc(1, sizeof(*reg_list) + DIV_ROUND_UP(reg_size, 8));
+		if (!reg_list) {
+			free(reg_value);
+			LOG_ERROR("Failed to allocated reg_list for %d-byte register.",
+					reg_size);
+			return ERROR_FAIL;
 		}
+		reg_list[0].number = reg_num;
+		reg_list[0].size = reg_size;
+		memcpy(&reg_list[0].value, reg_value, DIV_ROUND_UP(reg_size, 8));
+		free(reg_value);
+		num_regs = 1;
+	} else {
+		retval = target->rtos->type->get_thread_reg_list(target->rtos,
+				current_threadid,
+				&reg_list,
+				&num_regs);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("RTOS: failed to get register list");
+			return retval;
+		}
+	}
 
-		free(reg_list);
+	for (int i = 0; i < num_regs; ++i) {
+		if (reg_list[i].number == (uint32_t)reg_num) {
+			rtos_put_gdb_reg_list(connection, reg_list + i, 1);
+			free(reg_list);
+			return ERROR_OK;
+		}
 	}
+
+	free(reg_list);
+
 	return ERROR_FAIL;
 }
 
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 795f141eb0..a0a5e4af53 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -1397,8 +1397,13 @@ static int gdb_get_register_packet(struct connection *connection,
 	LOG_DEBUG("-");
 #endif
 
-	if ((target->rtos) && (rtos_get_gdb_reg(connection, reg_num) == ERROR_OK))
-		return ERROR_OK;
+	if (target->rtos) {
+		retval = rtos_get_gdb_reg(connection, reg_num);
+		if (retval == ERROR_OK)
+			return ERROR_OK;
+		if (retval != ERROR_NOT_IMPLEMENTED)
+			return gdb_error(connection, retval);
+	}
 
 	retval = target_get_gdb_reg_list_noread(target, &reg_list, &reg_list_size,
 			REG_CLASS_ALL);