diff --git a/gdbserver/gdb-server.c b/gdbserver/gdb-server.c
index f60aadc..789b609 100644
--- a/gdbserver/gdb-server.c
+++ b/gdbserver/gdb-server.c
@@ -192,6 +192,71 @@ winsock_error:
 	return 0;
 }
 
+static const char* const target_description_F4 =
+    "<?xml version=\"1.0\"?>"
+    "<!DOCTYPE target SYSTEM \"gdb-target.dtd\">"
+    "<target version=\"1.0\">"
+    "   <architecture>arm</architecture>"
+    "   <feature name=\"org.gnu.gdb.arm.m-profile\">"
+    "       <reg name=\"r0\" bitsize=\"32\"/>"
+    "       <reg name=\"r1\" bitsize=\"32\"/>"
+    "       <reg name=\"r2\" bitsize=\"32\"/>"
+    "       <reg name=\"r3\" bitsize=\"32\"/>"
+    "       <reg name=\"r4\" bitsize=\"32\"/>"
+    "       <reg name=\"r5\" bitsize=\"32\"/>"
+    "       <reg name=\"r6\" bitsize=\"32\"/>"
+    "       <reg name=\"r7\" bitsize=\"32\"/>"
+    "       <reg name=\"r8\" bitsize=\"32\"/>"
+    "       <reg name=\"r9\" bitsize=\"32\"/>"
+    "       <reg name=\"r10\" bitsize=\"32\"/>"
+    "       <reg name=\"r11\" bitsize=\"32\"/>"
+    "       <reg name=\"r12\" bitsize=\"32\"/>"
+    "       <reg name=\"sp\" bitsize=\"32\" type=\"data_ptr\"/>"
+    "       <reg name=\"lr\" bitsize=\"32\"/>"
+    "       <reg name=\"pc\" bitsize=\"32\" type=\"code_ptr\"/>"
+    "       <reg name=\"xpsr\" bitsize=\"32\" regnum=\"25\"/>"
+    "       <reg name=\"msp\" bitsize=\"32\" regnum=\"26\" type=\"data_ptr\" group=\"general\" />"
+    "       <reg name=\"psp\" bitsize=\"32\" regnum=\"27\" type=\"data_ptr\" group=\"general\" />"
+    "       <reg name=\"control\" bitsize=\"8\" regnum=\"28\" type=\"int\" group=\"general\" />"
+    "       <reg name=\"faultmask\" bitsize=\"8\" regnum=\"29\" type=\"int\" group=\"general\" />"
+    "       <reg name=\"basepri\" bitsize=\"8\" regnum=\"30\" type=\"int\" group=\"general\" />"
+    "       <reg name=\"primask\" bitsize=\"8\" regnum=\"31\" type=\"int\" group=\"general\" />"
+    "       <reg name=\"s0\" bitsize=\"32\" regnum=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s1\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s2\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s3\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s4\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s5\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s6\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s7\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s8\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s9\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s10\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s11\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s12\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s13\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s14\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s15\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s16\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s17\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s18\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s19\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s20\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s21\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s22\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s23\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s24\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s25\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s26\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s27\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s28\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s29\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s30\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"s31\" bitsize=\"32\" type=\"float\" group=\"float\" />"
+    "       <reg name=\"fpscr\" bitsize=\"32\" type=\"int\" group=\"float\" />"
+    "   </feature>"
+    "</target>";
+
 static const char* const memory_map_template_F4 =
   "<?xml version=\"1.0\"?>"
   "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
@@ -649,7 +714,12 @@ int serve(stlink_t *sl, int port) {
 			#endif
 
 			if(!strcmp(queryName, "Supported")) {
-				reply = strdup("PacketSize=3fff;qXfer:memory-map:read+");
+                if(sl->chip_id==STM32_CHIPID_F4) {
+                    reply = strdup("PacketSize=3fff;qXfer:memory-map:read+;qXfer:features:read+");
+                }
+                else {
+                    reply = strdup("PacketSize=3fff;qXfer:memory-map:read+");
+                }
 			} else if(!strcmp(queryName, "Xfer")) {
 				char *type, *op, *__s_addr, *s_length;
 				char *tok = params;
@@ -674,6 +744,9 @@ int serve(stlink_t *sl, int port) {
 				if(!strcmp(type, "memory-map") && !strcmp(op, "read"))
 					data = current_memory_map;
 
+				if(!strcmp(type, "features") && !strcmp(op, "read"))
+					data = target_description_F4;
+
 				if(data) {
 					unsigned data_length = strlen(data);
 					if(addr + length > data_length)
@@ -881,6 +954,30 @@ int serve(stlink_t *sl, int port) {
 			} else if(id == 0x19) {
 				stlink_read_reg(sl, 16, &regp);
 				myreg = htonl(regp.xpsr);
+			} else if(id == 0x1A) {
+				stlink_read_reg(sl, 17, &regp);
+				myreg = htonl(regp.main_sp);
+			} else if(id == 0x1B) {
+				stlink_read_reg(sl, 18, &regp);
+				myreg = htonl(regp.process_sp);
+			} else if(id == 0x1C) {
+				stlink_read_unsupported_reg(sl, id, &regp);
+				myreg = htonl(regp.control);
+			} else if(id == 0x1D) {
+				stlink_read_unsupported_reg(sl, id, &regp);
+				myreg = htonl(regp.faultmask);
+			} else if(id == 0x1E) {
+				stlink_read_unsupported_reg(sl, id, &regp);
+				myreg = htonl(regp.basepri);
+			} else if(id == 0x1F) {
+				stlink_read_unsupported_reg(sl, id, &regp);
+				myreg = htonl(regp.primask);
+            } else if(id >= 0x20 && id < 0x40) {
+                stlink_read_unsupported_reg(sl, id, &regp);
+                myreg = htonl(regp.s[id-0x20]);
+			} else if(id == 0x40) {
+                stlink_read_unsupported_reg(sl, id, &regp);
+                myreg = htonl(regp.fpscr);
 			} else {
 				reply = strdup("E00");
 			}
diff --git a/src/stlink-common.c b/src/stlink-common.c
index 359d39a..1e9c011 100644
--- a/src/stlink-common.c
+++ b/src/stlink-common.c
@@ -600,15 +600,24 @@ void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 }
 
 void stlink_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
+    int r_convert;
+
     DLOG("*** stlink_read_unsupported_reg\n");
     DLOG(" (%d) ***\n", r_idx);
 
-    if (r_idx != 0x21 || r_idx < 0x40 || r_idx > 0x5f) {
-        fprintf(stderr, "Error: register address must be in [0x21, 0x40..0x5f]\n");
+    /* Convert to values used by DCRSR */
+    if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
+        r_convert = 0x14;
+    } else if (r_idx == 0x40) {     /* FPSCR */
+        r_convert = 0x21;
+    } else if (r_idx >= 0x20 && r_idx < 0x40) {
+        r_convert = 0x40 + (r_idx - 0x20);
+    } else {
+        fprintf(stderr, "Error: register address must be in [0x1C..0x40]\n");
         return;
     }
 
-    sl->backend->read_unsupported_reg(sl, r_idx, regp);
+    sl->backend->read_unsupported_reg(sl, r_convert, regp);
 }
 
 unsigned int is_core_halted(stlink_t *sl) {
diff --git a/src/stlink-common.h b/src/stlink-common.h
index c982b3d..5a82e1f 100644
--- a/src/stlink-common.h
+++ b/src/stlink-common.h
@@ -244,6 +244,10 @@ extern "C" {
         uint32_t process_sp;
         uint32_t rw;
         uint32_t rw2;
+        uint8_t control;
+        uint8_t faultmask;
+        uint8_t basepri;
+        uint8_t primask;
         uint32_t fpscr;
     } reg;
 
diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index 397723c..fdd636b 100644
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -576,6 +576,7 @@ void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
     }
 }
 
+/* See section C1.6 of the ARMv7-M Architecture Reference Manual */
 void _stlink_usb_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
     uint32_t r;
 
@@ -591,6 +592,12 @@ void _stlink_usb_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
     DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
 
     switch (r_idx) {
+        case 0x14:
+            regp->primask = (uint8_t) (r & 0xFF);
+            regp->basepri = (uint8_t) ((r>>8) & 0xFF);
+            regp->faultmask = (uint8_t) ((r>>16) & 0xFF);
+            regp->control = (uint8_t) ((r>>24) & 0xFF);
+            break;
         case 0x21:
             regp->fpscr = r;
             break;