v3hil: fix for JTAGID=0x89 devices

v3hil now working properly with firmware versions 2.3.0.6, 3.8.0.2 and 3.12.0.0 of eZ-FET on EXP430FR5994
v3hil: eZ-FET 3.8(r) working; 3.12(y) and 3.15 TODO
2022-05-15 22:47:51 +02:00 · 2022-05-15 04:37:15 +02:00 · 2022-05-12 14:32:26 +02:00 · 2022-05-09 02:45:48 +02:00 · 2022-05-09 01:43:07 +02:00 · 2022-05-09 01:42:57 +02:00
22 changed files with 1774 additions and 3100 deletions
--- a/.gitignore
+++ b/.gitignore
@ -4,3 +4,5 @@ mspdebug
 mspdebug.exe
 inst/
 config.mk
+*.pcapng
+*.pcapng.gz
--- a/7
+++ b/7
@ -16,13 +16,13 @@
 # along with this program; if not, write to the Free Software
 # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

+-include config.mk
+
 CC ?= gcc
 INSTALL = /usr/bin/install
 PREFIX ?= /usr/local
 LDFLAGS ?= -s

-include config.mk
-
 BINDIR = ${PREFIX}/bin/
 MANDIR = ${PREFIX}/share/man/man1
 LIBDIR = ${PREFIX}/lib/
@ -197,9 +197,6 @@ OBJ=\
    drivers/jtdev_bus_pirate.o \
    drivers/jtdev_gpio.o \
    drivers/jtaglib.o \
-    drivers/jtaglib_cpu16.o \
-    drivers/jtaglib_cpux.o \
-    drivers/jtaglib_cpuxv2.o \
    drivers/mehfet_proto.o \
    drivers/mehfet.o \
    drivers/pif.o \
--- a/chipinfo.db
+++ b/chipinfo.db
@ -47335,7 +47335,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -47633,7 +47633,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -47931,7 +47931,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -48229,7 +48229,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -48527,7 +48527,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -48825,7 +48825,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -49123,7 +49123,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -49421,7 +49421,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -49719,7 +49719,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -50017,7 +50017,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -50315,7 +50315,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -50613,7 +50613,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -50911,7 +50911,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -51209,7 +51209,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -51507,7 +51507,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -51805,7 +51805,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -52103,7 +52103,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -52401,7 +52401,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -55325,7 +55325,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -60873,7 +60873,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -61171,7 +61171,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -61469,7 +61469,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -61767,7 +61767,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -62065,7 +62065,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -62363,7 +62363,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -62661,7 +62661,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -62959,7 +62959,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -63257,7 +63257,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -63555,7 +63555,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -63853,7 +63853,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -64151,7 +64151,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -64449,7 +64449,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -64747,7 +64747,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -65045,7 +65045,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -65343,7 +65343,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -65641,7 +65641,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -65939,7 +65939,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -66237,7 +66237,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -78736,7 +78736,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -78933,7 +78933,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -79130,7 +79130,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -79327,7 +79327,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -79524,7 +79524,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -79721,7 +79721,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -79918,7 +79918,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -80115,7 +80115,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -80312,7 +80312,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -80509,7 +80509,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -80706,7 +80706,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -80903,7 +80903,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -81100,7 +81100,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -81297,7 +81297,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -81494,7 +81494,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -81691,7 +81691,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -81888,7 +81888,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -82085,7 +82085,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -85045,7 +85045,7 @@ const struct chipinfo chipinfo_db[] = {	{
 		.id		= {
 			.ver_id		= 0x82a1,
 			.ver_sub_id	= 0x0000,
-			.revision	= 0x21/*0x10*/,
+			.revision	= /*0x10*/0x21,
 			.fab		= 0x00,
 			.self		= 0x0000,
 			.config		= 0x00,
@ -85084,7 +85084,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= /*0x50*/0x41,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -85314,7 +85314,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -85544,7 +85544,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -85774,7 +85774,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -85971,7 +85971,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -86168,7 +86168,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -86409,7 +86409,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -86650,7 +86650,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -86891,7 +86891,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -87132,7 +87132,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -87373,7 +87373,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -87592,7 +87592,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -87811,7 +87811,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -88030,7 +88030,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -88249,7 +88249,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -88490,7 +88490,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -88731,7 +88731,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -88950,7 +88950,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -90291,7 +90291,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -90906,7 +90906,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -91147,7 +91147,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -91388,7 +91388,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -91629,7 +91629,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -91870,7 +91870,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -93948,7 +93948,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
@ -94189,7 +94189,7 @@ const struct chipinfo chipinfo_db[] = {	{
 			[0x18]	= 0x3f,
 			[0x19]	= 0x3f,
 			[0x1a]	= 0x40,
-			[0x1c]	= 0x41/*0x50*/,
+			[0x1c]	= 0x50,
 			[0x1d]	= 0x42,
 			[0x1e]	= 0x43,
 			[0x1f]	= 0x44,
--- a/drivers/hal_proto.c
+++ b/drivers/hal_proto.c
@ -31,38 +31,10 @@ void hal_proto_init(struct hal_proto *p, transport_t trans,
 	p->ref_id = 0;
 }

-static int hal_proto_send_common(struct hal_proto *p, hal_proto_type_t type,
-		uint8_t* buf, size_t len)
-{
-	if (len & 1)
-		buf[len++] = 0;
-
-	if (p->flags & HAL_PROTO_CHECKSUM) {
-		size_t i;
-		uint8_t sum_l = 0xff;
-		uint8_t sum_h = 0xff;
-
-		for (i = 0; i < len; i += 2) {
-			sum_l ^= buf[i];
-			sum_h ^= buf[i + 1];
-		}
-
-		buf[len++] = sum_l;
-		buf[len++] = sum_h;
-	}
-
-	if (p->trans->ops->send(p->trans, buf, len) < 0) {
-		printc_err("hal_proto_send_common: type: 0x%02x\n", type);
-		return -1;
-	}
-
-	return 0;
-}
-
-
 int hal_proto_send(struct hal_proto *p, hal_proto_type_t type,
 		   const uint8_t *data, int length)
 {
+	//asm volatile("int3");
 	uint8_t buf[512];
 	size_t len = 0;

@ -81,13 +53,35 @@ int hal_proto_send(struct hal_proto *p, hal_proto_type_t type,
 	memcpy(buf + len, data, length);
 	len += length;

-	return hal_proto_send_common(p, type, buf, len);
+	if (len & 1)
+		buf[len++] = 0;
+
+	if (p->flags & HAL_PROTO_CHECKSUM) {
+		size_t i;
+		uint8_t sum_l = 0xff;
+		uint8_t sum_h = 0xff;
+
+		for (i = 0; i < len; i += 2) {
+			sum_l ^= buf[i];
+			sum_h ^= buf[i + 1];
+		}
+
+		buf[len++] = sum_l;
+		buf[len++] = sum_h;
+	}
+
+	if (p->trans->ops->send(p->trans, buf, len) < 0) {
+		printc_err("hal_proto_send: type: 0x%02x\n", type);
+		return -1;
+	}
+
+	return 0;
 }

-
 static int hal_proto_send_ack(struct hal_proto *p, hal_proto_type_t type,
-		const uint8_t *data, int length)
+		   const uint8_t *data, int length)
 {
+	//asm volatile("int3");
 	uint8_t buf[512];
 	size_t len = 0;

@ -101,17 +95,40 @@ static int hal_proto_send_ack(struct hal_proto *p, hal_proto_type_t type,
 	buf[len++] = (p->ref_id - 1) & 0xff;
 	buf[len++] = 0;

-	/*p->ref_id = (p->ref_id + 1) & 0x7f;*/
+	//p->ref_id = (p->ref_id + 1) & 0x7f;

 	memcpy(buf + len, data, length);
 	len += length;

-	return hal_proto_send_common(p, type, buf, len);
+	if (len & 1)
+		buf[len++] = 0;
+
+	if (p->flags & HAL_PROTO_CHECKSUM) {
+		size_t i;
+		uint8_t sum_l = 0xff;
+		uint8_t sum_h = 0xff;
+
+		for (i = 0; i < len; i += 2) {
+			sum_l ^= buf[i];
+			sum_h ^= buf[i + 1];
+		}
+
+		buf[len++] = sum_l;
+		buf[len++] = sum_h;
+	}
+
+	if (p->trans->ops->send(p->trans, buf, len) < 0) {
+		printc_err("hal_proto_send_ack: type: 0x%02x\n", type);
+		return -1;
+	}
+
+	return 0;
 }


 int hal_proto_receive(struct hal_proto *p, uint8_t *buf, int max_len)
 {
+	//asm volatile("int3");
 	uint8_t rx_buf[512];
 	uint8_t sum_h = 0xff;
 	uint8_t sum_l = 0xff;
@ -177,11 +194,11 @@ int hal_proto_receive(struct hal_proto *p, uint8_t *buf, int max_len)
 int hal_proto_execute(struct hal_proto *p, uint8_t fid,
 		      const uint8_t *data, int len)
 {
+	//asm volatile("int3");
 	uint8_t fdata[HAL_MAX_PAYLOAD];

 	if (len + 2 > HAL_MAX_PAYLOAD) {
-		printc_err("hal_proto_execute: fid 0x%02x: payload too big: %d\n",
-				fid, len);
+		printc_err("hal_proto_execute: fid 0x%02x: payload too big: %d\n", fid, len);
 		return -1;
 	}

--- a/drivers/hal_proto.h
+++ b/drivers/hal_proto.h
@ -39,7 +39,7 @@ typedef enum {
 	HAL_PROTO_TYPE_DCDC_RESTART		= 0x62,
 	HAL_PROTO_TYPE_CORE_SET_VCC		= 0x63,
 	HAL_PROTO_TYPE_CORE_GET_VCC		= 0x64,
-	HAL_PROTO_TYPE_CORE_SWITCH_FET		= 0x65,
+	HAL_PROTO_TYPE_CORE_SWITCH_FET	= 0x65,
 	HAL_PROTO_TYPE_CMP_VERSIONS		= 0x66,

 	HAL_PROTO_TYPE_CMD_LEGACY		= 0x7e,
@ -58,7 +58,7 @@ typedef enum {
 	HAL_PROTO_TYPE_CMD_PAUSE_LOOP		= 0x8c,
 	HAL_PROTO_TYPE_CMD_RESUME_LOOP		= 0x8d,
 	HAL_PROTO_TYPE_CMD_KILL_ALL		= 0x8e,
-	HAL_PROTO_TYPE_CMD_OVER_CURRENT		= 0x8f,
+	HAL_PROTO_TYPE_CMD_OVER_CURRENT = 0x8f,

 	HAL_PROTO_TYPE_ACKNOWLEDGE		= 0x91,
 	HAL_PROTO_TYPE_EXCEPTION		= 0x92,
@ -72,118 +72,119 @@ typedef enum {
 } hal_proto_flags_t;

 typedef enum {
-	HAL_PROTO_ERR_NONE	= 0x00,
-	HAL_PROTO_ERR_UNDEFINED	= 0xffff,
+	HAL_PROTO_ERR_NONE = 0x00,

-	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_RAM_START	= 0xFFFE,
-	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_RAM_SIZE	= 0xFFFD,
-	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_OFFSET		= 0xFFFC,
-	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_ADDRESS	= 0xFFFB,
-	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_LENGTH		= 0xFFFA,
-	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_TYPE		= 0xFFF9,
-	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_LOCKA		= 0xFFF8,
-	HAL_PROTO_ERR_EXECUTE_FUNCLET_EXECUTION_TIMEOUT	= 0xFFF7,
-	HAL_PROTO_ERR_EXECUTE_FUNCLET_EXECUTION_ERROR	= 0xFFF6,
+	HAL_PROTO_ERR_UNDEFINED = 0xffff,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_RAM_START =             0xFFFE,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_RAM_SIZE =              0xFFFD,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_OFFSET =                0xFFFC,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_ADDRESS =               0xFFFB,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_LENGTH =                0xFFFA,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_TYPE =                  0xFFF9,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_LOCKA =                 0xFFF8,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_EXECUTION_TIMEOUT =        0xFFF7,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_EXECUTION_ERROR =          0xFFF6,

-	HAL_PROTO_ERR_WRITE_MEM_WORD_NO_RAM_ADDRESS	= 0xFFF5,
-	HAL_PROTO_ERR_WRITE_MEM_WORD_NO_RAM_SIZE	= 0xFFF4,
-	HAL_PROTO_ERR_WRITE_MEM_WORD_UNKNOWN		= 0xFFF3,
+	HAL_PROTO_ERR_WRITE_MEM_WORD_NO_RAM_ADDRESS =              0xFFF5,
+	HAL_PROTO_ERR_WRITE_MEM_WORD_NO_RAM_SIZE =                 0xFFF4,
+	HAL_PROTO_ERR_WRITE_MEM_WORD_UNKNOWN =                     0xFFF3,

-	HAL_PROTO_ERR_WRITE_MEM_BYTES_NO_RAM_ADDRESS	= 0xFFF2,
-	HAL_PROTO_ERR_WRITE_MEM_BYTES_NO_RAM_SIZE	= 0xFFF1,
-	HAL_PROTO_ERR_WRITE_MEM_BYTES_UNKNOWN		= 0xFFF0,
+	HAL_PROTO_ERR_WRITE_MEM_BYTES_NO_RAM_ADDRESS =             0xFFF2,
+	HAL_PROTO_ERR_WRITE_MEM_BYTES_NO_RAM_SIZE =                0xFFF1,
+	HAL_PROTO_ERR_WRITE_MEM_BYTES_UNKNOWN =                    0xFFF0,

-	HAL_PROTO_ERR_WRITE_FLASH_WORD_NO_FLASH_ADDRESS	= 0xFFEF,
-	HAL_PROTO_ERR_WRITE_FLASH_WORD_NO_FLASH_SIZE	= 0xFFEE,
-	HAL_PROTO_ERR_WRITE_FLASH_WORD_UNKNOWN		= 0xFFED,
+	HAL_PROTO_ERR_WRITE_FLASH_WORD_NO_FLASH_ADDRESS =          0xFFEF,
+	HAL_PROTO_ERR_WRITE_FLASH_WORD_NO_FLASH_SIZE =             0xFFEE,
+	HAL_PROTO_ERR_WRITE_FLASH_WORD_UNKNOWN =                   0xFFED,

-	HAL_PROTO_ERR_WRITE_FLASH_QUICK_UNKNOWN		= 0xFFEC,
+	HAL_PROTO_ERR_WRITE_FLASH_QUICK_UNKNOWN =                  0xFFEC,

-	HAL_PROTO_ERR_START_JTAG_NO_PROTOCOL		= 0xFFEB,
-	HAL_PROTO_ERR_START_JTAG_PROTOCOL_UNKNOWN	= 0xFFEA,
+	HAL_PROTO_ERR_START_JTAG_NO_PROTOCOL =                     0xFFEB,
+	HAL_PROTO_ERR_START_JTAG_PROTOCOL_UNKNOWN =                0xFFEA,

-	HAL_PROTO_ERR_SET_CHAIN_CONFIGURATION_STREAM	= 0xFFE9,
+	HAL_PROTO_ERR_SET_CHAIN_CONFIGURATION_STREAM =             0xFFE9,

-	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_WDT_ADDRESS	= 0xFFE8,
-	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_WDT_VALUE		= 0xFFE7,
-	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_PC			= 0xFFE6,
-	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_SR			= 0xFFE5,
-	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_CONTROL_MASK	= 0xFFE4,
-	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_MDB		= 0xFFE3,
+	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_WDT_ADDRESS = 0xFFE8,
+	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_WDT_VALUE =   0xFFE7,
+	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_PC =          0xFFE6,
+	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_SR =          0xFFE5,
+	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_CONTROL_MASK =0xFFE4,
+	HAL_PROTO_ERR_RESTORECONTEXT_RELEASE_JTAG_NO_MDB =         0xFFE3,

-	HAL_PROTO_ERR_READ_MEM_WORD_NO_ADDRESS	= 0xFFF2,
-	HAL_PROTO_ERR_READ_MEM_WORD_NO_SIZE	= 0xFFF1,
+	HAL_PROTO_ERR_READ_MEM_WORD_NO_ADDRESS =                   0xFFF2,
+	HAL_PROTO_ERR_READ_MEM_WORD_NO_SIZE =                      0xFFF1,

-	HAL_PROTO_ERR_READ_MEM_UNKNOWN		= 0xFFE0,
+	HAL_PROTO_ERR_READ_MEM_UNKNOWN =                           0xFFE0,

-	HAL_PROTO_ERR_READ_MEM_BYTES_NO_ADDRESS	= 0xFFDF,
-	HAL_PROTO_ERR_READ_MEM_BYTES_NO_SIZE	= 0xFFDE,
+	HAL_PROTO_ERR_READ_MEM_BYTES_NO_ADDRESS =                  0xFFDF,
+	HAL_PROTO_ERR_READ_MEM_BYTES_NO_SIZE =                     0xFFDE,

-	HAL_PROTO_ERR_PSA_NO_ADDRESS		= 0xFFDD,
-	HAL_PROTO_ERR_PSA_NO_SIZE		= 0xFFDC,
+	HAL_PROTO_ERR_PSA_NO_ADDRESS =                             0xFFDD,
+	HAL_PROTO_ERR_PSA_NO_SIZE =                                0xFFDC,

-	HAL_PROTO_ERR_SYNC_JTAG_ASSERT_POR_JTAG_TIMEOUT		= 0xFFDB,
-	HAL_PROTO_ERR_SYNC_JTAG_ASSERT_POR_NO_WDT_ADDRESS	= 0xFFDA,
-	HAL_PROTO_ERR_SYNC_JTAG_ASSERT_POR_NO_WDT_VALUE		= 0xFFD9,
+	HAL_PROTO_ERR_SYNC_JTAG_ASSERT_POR_JTAG_TIMEOUT =          0xFFDB,
+	HAL_PROTO_ERR_SYNC_JTAG_ASSERT_POR_NO_WDT_ADDRESS =        0xFFDA,
+	HAL_PROTO_ERR_SYNC_JTAG_ASSERT_POR_NO_WDT_VALUE =          0xFFD9,

-	HAL_PROTO_ERR_WRITE_ALL_CPU_REGISTERS_STREAM	= 0xFFD8,
+	HAL_PROTO_ERR_WRITE_ALL_CPU_REGISTERS_STREAM =             0xFFD8,

-	HAL_PROTO_ERR_WRITE_MEM_WORD_XV2_NO_RAM_ADDRESS	= 0xFFD7,
-	HAL_PROTO_ERR_WRITE_MEM_WORD_XV2_NO_RAM_SIZE	= 0xFFD6,
+	HAL_PROTO_ERR_WRITE_MEM_WORD_XV2_NO_RAM_ADDRESS =          0xFFD7,
+	HAL_PROTO_ERR_WRITE_MEM_WORD_XV2_NO_RAM_SIZE =             0xFFD6,

-	HAL_PROTO_ERR_SECURE_NO_TGT_HAS_TEST_PIN	= 0xFFD5,
+	HAL_PROTO_ERR_SECURE_NO_TGT_HAS_TEST_PIN =                 0xFFD5,

-	HAL_PROTO_ERR_SYNC_JTAG_CONDITIONAL_JTAG_TIMEOUT	= 0xFFD4,
-	HAL_PROTO_ERR_SYNC_JTAG_CONDITIONAL_NO_WDT_ADDRESS	= 0xFFD3,
-	HAL_PROTO_ERR_SYNC_JTAG_CONDITIONAL_NO_WDT_VALUE	= 0xFFD2,
+	HAL_PROTO_ERR_SYNC_JTAG_CONDITIONAL_JTAG_TIMEOUT =         0xFFD4,
+	HAL_PROTO_ERR_SYNC_JTAG_CONDITIONAL_NO_WDT_ADDRESS =       0xFFD3,
+	HAL_PROTO_ERR_SYNC_JTAG_CONDITIONAL_NO_WDT_VALUE =         0xFFD2,

-	HAL_PROTO_ERR_INSTRUCTION_BOUNDARY_ERROR	= 0xFFD1,
-	HAL_PROTO_ERR_JTAG_VERSION_MISMATCH		= 0xFFD0,
+	HAL_PROTO_ERR_INSTRUCTION_BOUNDARY_ERROR =                 0xFFD1,

-	HAL_PROTO_ERR_JTAG_MAILBOX_IN_TIMOUT		= 0xFFCF,
-	HAL_PROTO_ERR_JTAG_PASSWORD_WRONG		= 0xFFCE,
+	HAL_PROTO_ERR_JTAG_VERSION_MISMATCH =                      0xFFD0,

-	HAL_PROTO_ERR_START_JTAG_NO_ACTIVATION_CODE	= 0xFFCD,
-	HAL_PROTO_ERR_SINGLESTEP_WAITFOREEM_TIMEOUT	= 0xFFCC,
+	HAL_PROTO_ERR_JTAG_MAILBOX_IN_TIMOUT =                     0xFFCF,
+	HAL_PROTO_ERR_JTAG_PASSWORD_WRONG =                        0xFFCE,

-	HAL_PROTO_ERR_CONFIG_NO_PARAMETER		= 0xFFCB,
-	HAL_PROTO_ERR_CONFIG_NO_VALUE			= 0xFFCA,
-	HAL_PROTO_ERR_CONFIG_PARAM_UNKNOWN_PARAMETER	= 0xFFC9,
+	HAL_PROTO_ERR_START_JTAG_NO_ACTIVATION_CODE =              0xFFCD,
+	HAL_PROTO_ERR_SINGLESTEP_WAITFOREEM_TIMEOUT =              0xFFCC,

-	HAL_PROTO_ERR_NO_NUM_BITS			= 0xFFC8,
-	HAL_PROTO_ERR_ARRAY_SIZE_MISMATCH		= 0xFFC7,
+	HAL_PROTO_ERR_CONFIG_NO_PARAMETER =                        0xFFCB,
+	HAL_PROTO_ERR_CONFIG_NO_VALUE =                            0xFFCA,
+	HAL_PROTO_ERR_CONFIG_PARAM_UNKNOWN_PARAMETER =             0xFFC9,

-	HAL_PROTO_ERR_NO_COMMAND			= 0xFFC6,
-	HAL_PROTO_ERR_UNKNOWN_COMMAND			= 0xFFC5,
-	HAL_PROTO_ERR_NO_DATA				= 0xFFC4,
-	HAL_PROTO_ERR_NO_BIT_SIZE			= 0xFFC3,
-	HAL_PROTO_ERR_INVALID_BIT_SIZE			= 0xFFC2,
+	HAL_PROTO_ERR_NO_NUM_BITS =                                0xFFC8,
+	HAL_PROTO_ERR_ARRAY_SIZE_MISMATCH =                        0xFFC7,

-	HAL_PROTO_ERR_UNLOCK_NO_PASSWORD_LENGTH		= 0xFFC1,
-	HAL_PROTO_ERR_UNLOCK_INVALID_PASSWORD_LENGTH	= 0xFFC0,
+	HAL_PROTO_ERR_NO_COMMAND =                                 0xFFC6,
+	HAL_PROTO_ERR_UNKNOWN_COMMAND =                            0xFFC5,
+	HAL_PROTO_ERR_NO_DATA =                                    0xFFC4,
+	HAL_PROTO_ERR_NO_BIT_SIZE =                                0xFFC3,
+	HAL_PROTO_ERR_INVALID_BIT_SIZE =                           0xFFC2,

-	HAL_PROTO_ERR_EXECUTE_FUNCLET_FINISH_TIMEOUT	= 0xFFBF,
+	HAL_PROTO_ERR_UNLOCK_NO_PASSWORD_LENGTH =                  0xFFC1,
+	HAL_PROTO_ERR_UNLOCK_INVALID_PASSWORD_LENGTH =             0xFFC0,

-	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_MAXRSEL	= 0xFFBE,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_FINISH_TIMEOUT =             0xFFBF,

-	HAL_PROTO_ERR_API_CALL_NOT_SUPPORTED		= 0xFFBD,
+	HAL_PROTO_ERR_EXECUTE_FUNCLET_NO_MAXRSEL =                 0xFFBE,

-	HAL_PROTO_ERR_MAGIC_PATTERN			= 0xFFBC,
-	HAL_PROTO_ERR_MAGIC_PATTERN_BOOT_DATA_CRC_WRONG	= 0xFFBB,
-	HAL_PROTO_ERR_DAP_NACK				= 0xFFBA,
+	HAL_PROTO_ERR_API_CALL_NOT_SUPPORTED =                     0xFFBD,

-	HAL_PROTO_MESSAGE_NO_RESPONSE		= 0x8000,
-	HAL_PROTO_EXCEPTION_NOT_IMPLEMENT_ERR	= 0x8001,
-	HAL_PROTO_EXCEPTION_MSGID_ERR		= 0x8002,
-	HAL_PROTO_EXCEPTION_CRC_ERR		= 0x8003,
-	HAL_PROTO_EXCEPTION_RX_TIMEOUT_ERR	= 0x8004,
-	HAL_PROTO_EXCEPTION_TX_TIMEOUT_ERR	= 0x8005,
-	HAL_PROTO_EXCEPTION_RX_OVERFLOW_ERR	= 0x8006,
-	HAL_PROTO_EXCEPTION_TX_NO_BUFFER	= 0x8007,
-	HAL_PROTO_EXCEPTION_COM_RESET		= 0x8008,
-	HAL_PROTO_EXCEPTION_RX_NO_BUFFER	= 0x8009,
-	HAL_PROTO_EXCEPTION_RX_TO_SMALL_BUFFER	= 0x800A,
-	HAL_PROTO_EXCEPTION_RX_LENGTH		= 0x800B,
+	HAL_PROTO_ERR_MAGIC_PATTERN =                              0xFFBC,
+	HAL_PROTO_ERR_MAGIC_PATTERN_BOOT_DATA_CRC_WRONG =          0xFFBB,
+	HAL_PROTO_ERR_DAP_NACK =                                   0xFFBA,
+
+	HAL_PROTO_MESSAGE_NO_RESPONSE           = 0x8000,
+	HAL_PROTO_EXCEPTION_NOT_IMPLEMENT_ERR   = 0x8001,
+	HAL_PROTO_EXCEPTION_MSGID_ERR           = 0x8002,
+	HAL_PROTO_EXCEPTION_CRC_ERR             = 0x8003,
+	HAL_PROTO_EXCEPTION_RX_TIMEOUT_ERR      = 0x8004,
+	HAL_PROTO_EXCEPTION_TX_TIMEOUT_ERR      = 0x8005,
+	HAL_PROTO_EXCEPTION_RX_OVERFLOW_ERR     = 0x8006,
+	HAL_PROTO_EXCEPTION_TX_NO_BUFFER        = 0x8007,
+	HAL_PROTO_EXCEPTION_COM_RESET           = 0x8008,
+	HAL_PROTO_EXCEPTION_RX_NO_BUFFER        = 0x8009,
+	HAL_PROTO_EXCEPTION_RX_TO_SMALL_BUFFER  = 0x800A,
+	HAL_PROTO_EXCEPTION_RX_LENGTH           = 0x800B,
 } hal_proto_error_t;

 #define HAL_MAX_PAYLOAD		253
--- a/drivers/jtaglib.c
+++ b/drivers/jtaglib.c
--- a/drivers/jtaglib.h
+++ b/drivers/jtaglib.h
@ -34,56 +34,16 @@
 #ifndef JTAGLIB_H_
 #define JTAGLIB_H_

-#include <stdbool.h>
 #include <stdint.h>

 #include "device.h"
 #include "jtdev.h"
 #include "util.h"

-/* Colleciton of functions that need different implementation for different
- * CPU types (that is, 16-bit vs CPUX vs Xv2) */
-struct jtaglib_funcs {
-	unsigned int (*jlf_get_device)(struct jtdev *p);
-
-	uint16_t (*jlf_read_mem)(struct jtdev *p, unsigned int format, address_t address);
-	void (*jlf_read_mem_quick)(struct jtdev *p, address_t start_address,
-			unsigned int word_count, uint16_t* data);
-	void (*jlf_write_mem)(struct jtdev *p, unsigned int format,
-			address_t address, uint16_t data);
-	void (*jlf_write_mem_quick)(struct jtdev *p, address_t start_address,
-			unsigned int word_count, const uint16_t* data);
-
-	unsigned int (*jlf_execute_puc)(struct jtdev *p);
-	void (*jlf_release_device)(struct jtdev *p, address_t address);
-
-	int (*jlf_verify_mem)(struct jtdev *p, address_t start_address,
-			unsigned int length, const uint16_t *data);
-	int (*jlf_erase_check)(struct jtdev *p, address_t start_address,
-			unsigned int length);
-
-	void (*jlf_write_flash)(struct jtdev *p, address_t start_address,
-			unsigned int word_count, const uint16_t *data);
-	void (*jlf_erase_flash)(struct jtdev *p, unsigned int erase_mode,
-			address_t erase_address);
-
-	address_t (*jlf_read_reg)(struct jtdev *p, int reg);
-	void (*jlf_write_reg)(struct jtdev *p, int reg, address_t value);
-	void (*jlf_single_step)(struct jtdev *p);
-	unsigned int (*jlf_set_breakpoint)(struct jtdev *p, int bp_num,
-			address_t bp_addr);
-	unsigned int (*jlf_cpu_state)(struct jtdev *p);
-	int (*jlf_get_config_fuses)(struct jtdev *p);
-
-	void (*jlf_context_save)(struct jtdev *p, bool after_puc);
-	void (*jlf_context_restore)(struct jtdev *p);
-	void (*jlf_regs_update)(struct jtdev *p);
-	void (*jlf_regs_flush)(struct jtdev *p);
-};
-
-extern const struct jtaglib_funcs jlf_cpu16;
-extern const struct jtaglib_funcs jlf_cpux;
-extern const struct jtaglib_funcs jlf_cpuxv2;
+/* Flash erasing modes */
+#define JTAG_ERASE_MASS 0xA506
+#define JTAG_ERASE_MAIN 0xA504
+#define JTAG_ERASE_SGMT 0xA502

 /* Take target device under JTAG control. */
 unsigned int jtag_init(struct jtdev *p);
@ -158,35 +118,14 @@ unsigned int jtag_set_breakpoint(struct jtdev *p,
 unsigned int jtag_cpu_state(struct jtdev *p);
 int jtag_get_config_fuses(struct jtdev *p);

-void jtag_dev_context_save(struct jtdev *p, bool after_puc);
-void jtag_dev_context_restore(struct jtdev *p);
-void jtag_dev_regs_update(struct jtdev *p);
-void jtag_dev_regs_flush(struct jtdev *p);
-
 /* Default low-level JTAG routines for jtdev implementations that don't have
 * their own implementations of these routines */
 uint8_t jtag_default_ir_shift(struct jtdev *p, uint8_t ir);
 uint8_t jtag_default_dr_shift_8(struct jtdev *p, uint8_t dr);
 uint16_t jtag_default_dr_shift_16(struct jtdev *p, uint16_t dr);
-uint32_t jtag_default_dr_shift_20(struct jtdev *p, uint32_t dr);
 void jtag_default_tms_sequence(struct jtdev *p, int bits, unsigned int value);
 void jtag_default_init_dap(struct jtdev *p);

-void jtag_dev_default_context_save(struct jtdev *p, bool after_puc);
-void jtag_dev_default_context_restore(struct jtdev *p);
-void jtag_dev_default_regs_update(struct jtdev *p);
-void jtag_dev_default_regs_flush(struct jtdev *p);
-
-int jtag_refresh_bps(device_t dev, struct jtdev *p);
-
-int jtag_dev_readmem(device_t dev_base, address_t addr, uint8_t *mem, address_t len);
-int jtag_dev_writemem(device_t dev_base, address_t addr, const uint8_t *mem, address_t len);
-int jtag_dev_getregs(device_t dev_base, address_t *regs);
-int jtag_dev_setregs(device_t dev_base, const address_t *regs);
-int jtag_dev_ctl(device_t dev_base, device_ctl_t type);
-device_status_t jtag_dev_poll(device_t dev_base);
-int jtag_dev_erase(device_t dev_base, device_erase_type_t, address_t addr);
-int jtag_dev_getconfigfuses(device_t dev_base);
-int jtag_dev_init(struct jtdev *p);
+int jtag_refresh_bps(const char *driver, device_t dev, struct jtdev *p);

 #endif
--- a/drivers/jtaglib_cpu16.c
+++ b/drivers/jtaglib_cpu16.c
@ -1,883 +0,0 @@
-
-#include "jtaglib.h"
-#include "jtaglib_defs.h"
-#include "output.h"
-
-#ifdef DEBUG_JTAGLIB
-#define dbg_printc(fmt, ...) printc_dbg("jlf16: %s:%d " fmt, __func__, __LINE__, ##__VA_ARGS__)
-#else
-#define dbg_printc(fmt, ...) do{}while(0)
-#endif
-
-/* Set target CPU JTAG state machine into the instruction fetch state
- * return: 1 - instruction fetch was set
- *         0 - otherwise
- */
-static int jlf16_set_instruction_fetch(struct jtdev *p)
-{	// SLAU320AJ name: SetInstrFetch / SyncJtag?
-	unsigned int loop_counter;
-
-	// SyncJtag: has CTLR_SIG_16BIT=0x2401 here
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	/* Wait until CPU is in instruction fetch state
-	 * timeout after limited attempts
-	 */
-	for (loop_counter = 50; loop_counter > 0; loop_counter--) {
-		if ((jtag_dr_shift_16(p, 0x0000) & 0x0080) == 0x0080)
-			return 1;
-
-		jtag_tclk_clr(p); /* The TCLK pulse befor jtag_dr_shift_16 leads to   */
-		jtag_tclk_set(p); /* problems at MEM_QUICK_READ, it's from SLAU265 */
-	}
-
-	printc_err("jlf16_set_instruction_fetch: failed\n");
-	p->failed = 1;
-
-	return 0;
-}
-
-/* Set the CPU into a controlled stop state */
-static void jlf16_halt_cpu(struct jtdev *p)
-{	// SLAU320AJ name: HaltCPU
-	/* Set CPU into instruction fetch mode */
-	dbg_printc("halt cpu\n");
-
-	jlf16_set_instruction_fetch(p);
-
-	/* Set device into JTAG mode + read */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2401);
-
-	/* Send JMP $ instruction to keep CPU from changing the state */
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_dr_shift_16(p, 0x3FFF);
-	jtag_tclk_set(p); // TODO: ???
-	jtag_tclk_clr(p);
-
-	/* Set JTAG_HALT bit */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2409);
-	jtag_tclk_set(p);
-}
-
-/* Release the target CPU from the controlled stop state */
-static void jlf16_release_cpu(struct jtdev *p)
-{	// SLAU320AJ name: ReleaseCPU
-	dbg_printc("release cpu\n");
-
-	jtag_tclk_clr(p);
-
-	/* clear the HALT_JTAG bit */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2401);
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	jtag_tclk_set(p);
-}
-
-/* Compares the computed PSA (Pseudo Signature Analysis) value to the PSA
- * value shifted out from the target device. It is used for very fast data
- * block write or erasure verification.
- * start_address: start of data
- * length       : number of data
- * data         : pointer to data, 0 for erase check
- * RETURN       : 1 - comparison was successful
- *                0 - otherwise
- */
-static int jlf16_verify_mem(struct jtdev *p,
-			   unsigned int start_address,
-			   unsigned int length,
-			   const uint16_t *data)
-{	// SLAU320AJ name: VerifyMem/VerifyPSA
-	unsigned int psa_value;
-	unsigned int index;
-
-	/* Polynom value for PSA calculation */
-	unsigned int polynom = 0x0805;
-	/* Start value for PSA calculation */
-	unsigned int psa_crc = start_address-2;
-
-	dbg_printc("verify: %04x..%04x\n", start_address, start_address+length*2);
-
-	jtag_execute_puc(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2401);
-	jlf16_set_instruction_fetch(p);
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_dr_shift_16(p, 0x4030);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_dr_shift_16(p, start_address-2);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	jtag_dr_shift_16(p, 0x0000);
-	jtag_ir_shift(p, IR_DATA_PSA);
-
-	for (index = 0; index < length; index++) {
-		/* Calculate the PSA value */
-		if ((psa_crc & 0x8000) == 0x8000) {
-			psa_crc  ^= polynom;
-			psa_crc <<= 1;
-			psa_crc  |= 0x0001;
-		} else
-			psa_crc <<= 1;
-
-		if (data == 0)
-			/* use erase check mask */
-			psa_crc ^= 0xFFFF;
-		else
-			/* use data */
-			psa_crc ^= data[index];
-
-		/* Clock through the PSA */
-		jtag_tclk_set(p);
-
-		/* Go through DR path without shifting data in/out */
-		jtag_tms_sequence(p, 6, 0x19); /* TMS=1 0 0 1 1 0 ; 6 clocks */
-
-		jtag_tclk_clr(p);
-	}
-
-	/* Read out the PSA value */
-	jtag_ir_shift(p, IR_SHIFT_OUT_PSA);
-	psa_value = jtag_dr_shift_16(p, 0x0000);
-	jtag_tclk_set(p);
-
-	return (psa_value == psa_crc) ? 1 : 0;
-}
-
-/* ------------------------------------------------------------------------- */
-
-static int jlf16_erase_check(struct jtdev *p, unsigned int start_address,
-			   unsigned int length)
-{
-	return jlf16_verify_mem(p, start_address, length, NULL);
-}
-
-static unsigned int jlf16_get_device(struct jtdev *p)
-{	// SLAU320AJ name: GetDevice
-	unsigned int jtag_id = 0;
-	unsigned int loop_counter;
-
-	/* Set device into JTAG mode + read */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2401);
-
-	/* Wait until CPU is synchronized,
-	 * timeout after a limited number of attempts
-	 */
-	jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	for ( loop_counter = 50; loop_counter > 0; loop_counter--) {
-		if ( (jtag_dr_shift_16(p, 0x0000) & 0x0200) == 0x0200 ) {
-			break;
-		}
-	}
-
-	dbg_printc("get device: jtag id=%02x\n", jtag_id);
-
-	if (loop_counter == 0) {
-		printc_err("jlf16_get_device: timed out\n");
-		p->failed = 1;
-		/* timeout reached */
-		return 0;
-	}
-
-	return jtag_id;
-}
-
-/* Reads one byte/word from a given address
- * format : 8-byte, 16-word
- * address: address of memory
- * return : content of memory
- */
-static uint16_t jlf16_read_mem(struct jtdev *p, unsigned int format, address_t address)
-{	// SLAU320AJ name: ReadMem
-	uint16_t content;
-
-	dbg_printc("%dbit %04x\n", format, address);
-
-	jlf16_halt_cpu(p);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	if (format == 16) {
-		/* set word read */
-		jtag_dr_shift_16(p, 0x2409);
-	} else {
-		/* set byte read */
-		jtag_dr_shift_16(p, 0x2419);
-	}
-	/* set address */
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_16(p, address);
-	jtag_ir_shift(p, IR_DATA_TO_ADDR);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-
-	/* shift out 16 bits */
-	content = jtag_dr_shift_16(p, 0x0000);
-	jtag_tclk_set(p); /* is also the first instruction in jtag_release_cpu() */
-	jlf16_release_cpu(p);
-	if (format == 8)
-		content &= 0x00ff;
-
-	dbg_printc("%dbit %04x -> %04x\n", format, address, content);
-
-	return content;
-}
-
-/* Reads an array of words from target memory
- * address: address to read from
- * length : number of word to read
- * data   : memory to write to
- */
-static void jlf16_read_mem_quick(struct jtdev *p, address_t address,
-			 unsigned int length, uint16_t *data)
-{	// SLAU320AJ name: ReadMemQuick
-	unsigned int index;
-	address_t pc_bak;
-
-	dbg_printc("%04x..%04x\n", address, address+length*2);
-
-	pc_bak = jtag_read_reg(p, 0);
-
-	/* Initialize reading: */
-	jtag_write_reg(p, 0, address-4);
-	jlf16_halt_cpu(p);
-	jtag_tclk_clr(p);
-
-	/* set RW to read */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2409);
-	jtag_ir_shift(p, IR_DATA_QUICK);
-
-	for (index = 0; index < length; index++) {
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p); // TODO: ???
-		/* shift out the data from the target */
-		data[index] = jtag_dr_shift_16(p, 0x0000);
-		//jtag_tclk_clr(p); // TODO: ???
-	}
-
-	jtag_tclk_set(p);
-	jlf16_release_cpu(p);
-	jtag_write_reg(p, 0, pc_bak);
-}
-
-/* Writes one byte/word at a given address
- * format : 8-byte, 16-word
- * address: address to be written
- * data   : data to write
- */
-static void jlf16_write_mem(struct jtdev *p, unsigned int format,
-		    address_t address, uint16_t data)
-{	// SLAU320AJ name: WriteMem
-	dbg_printc("%dbit %04x <- %04x\n", format, address, data);
-
-	jlf16_halt_cpu(p);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-
-	if (format == 16)
-		/* Set word write */
-		jtag_dr_shift_16(p, 0x2408);
-	else
-		/* Set byte write */
-		jtag_dr_shift_16(p, 0x2418);
-
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-
-	/* Set addr */
-	jtag_dr_shift_16(p, address);
-	jtag_ir_shift(p, IR_DATA_TO_ADDR);
-
-	/* Shift in 16 bits */
-	jtag_dr_shift_16(p, data);
-	jtag_tclk_set(p);
-	jlf16_release_cpu(p);
-}
-
-/* Writes an array of words into target memory
- * address: address to write to
- * length : number of word to write
- * data   : data to write
- */
-static void jlf16_write_mem_quick(struct jtdev *p, address_t address,
-			  unsigned int length, const uint16_t *data)
-{	// SLAU320AJ name: WriteMemQuick
-	unsigned int index;
-
-	dbg_printc("%04x..%04x\n", address, address+length*2);
-
-	/* Initialize writing */
-	jtag_write_reg(p, 0, address-4);
-	jlf16_halt_cpu(p);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-
-	/* Set RW to write */
-	jtag_dr_shift_16(p, 0x2408);
-	jtag_ir_shift(p, IR_DATA_QUICK);
-	// TODO: ^ in start of loop?
-
-	for (index = 0; index < length; index++) {
-		/* Write data */
-		jtag_dr_shift_16(p, data[index]);
-
-		/* Increment PC by 2 */
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-	}
-
-	jtag_tclk_set(p);
-	jlf16_release_cpu(p);
-}
-
-/* Execute a Power-Up Clear (PUC) using JTAG CNTRL SIG register
- * return: JTAG ID
- */
-static unsigned int jlf16_execute_puc(struct jtdev *p)
-{	// SLAU320AJ name: ExecutePOR
-	unsigned int jtag_id;
-
-	dbg_printc("\n");
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-
-	/* Apply and remove reset */
-	jtag_dr_shift_16(p, 0x2C01);
-	jtag_dr_shift_16(p, 0x2401);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p); // TODO: ???
-
-	/* Read jtag id */
-	jtag_id = jtag_ir_shift(p, IR_ADDR_CAPTURE);
-
-	//jtag_tclk_set(p); // TODO: ???
-
-	/* Disable watchdog on target device */
-	//jtag_write_mem(p, 16, 0x0120, 0x5A80); // FIXME
-
-	return jtag_id;
-}
-
-/* Release the target device from JTAG control
- * address: 0xFFFE - perform Reset,
- *                   load Reset Vector into PC
- *          0xFFFF - start execution at current
- *                   PC position
- *          other  - load Address into PC
- */
-static void jlf16_release_device(struct jtdev *p, address_t address)
-{	// SLAU320AJ name: ReleaseDevice
-	switch (address) {
-		case 0xffff: /* Nothing to do */
-			dbg_printc("BOR\n");
-			break;
-		case 0xfffe: /* Perform reset */
-			dbg_printc("SRST\n");
-			/* delete all breakpoints */
-			jtag_set_breakpoint(p,-1,0);
-			/* issue reset */
-			jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-			jtag_dr_shift_16(p, 0x2C01);
-			jtag_dr_shift_16(p, 0x2401);
-			break;
-		default: /* Set target CPU's PC */
-			dbg_printc("PC: %04x\n", address);
-			jtag_write_reg(p, 0, address);
-			break;
-	}
-
-	jlf16_set_instruction_fetch(p);
-
-	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE);
-	jtag_dr_shift_16(p, BREAKREACT + READ);
-	jtag_dr_shift_16(p, 0x0000);
-
-	jtag_ir_shift(p, IR_EMEX_WRITE_CONTROL);
-	jtag_dr_shift_16(p, 0x000f);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_RELEASE);
-}
-
-/* Programs/verifies an array of words into a FLASH by using the
- * FLASH controller. The JTAG FLASH register isn't needed.
- * start_address: start in FLASH
- * length       : number of words
- * data         : pointer to data
- */
-static void jlf16_write_flash(struct jtdev *p, address_t start_address,
-		      unsigned int length, const uint16_t *data)
-{	// SLAU320AJ name: WriteFLASH
-	unsigned int index;
-	unsigned int address;
-
-	dbg_printc("%04x..%04x\n", address, address+length*2);
-
-	address = start_address;
-	jlf16_halt_cpu(p);
-	jtag_tclk_clr(p);
-
-	/* Set RW to write */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2408);
-
-	/* FCTL1 register */
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_16(p, 0x0128); // FIXME
-
-	/* Enable FLASH write */
-	jtag_ir_shift(p, IR_DATA_TO_ADDR);
-	jtag_dr_shift_16(p, 0xA540);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-
-	/* FCTL2 register */
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_16(p, 0x012A); // FIXME
-
-	/* Select MCLK as source, DIV=1 */
-	jtag_ir_shift(p, IR_DATA_TO_ADDR);
-	jtag_dr_shift_16(p, 0xA540);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-
-	/* FCTL3 register */
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_16(p, 0x012C); // FIXME
-
-	/* Clear FCTL3 register */
-	jtag_ir_shift(p, IR_DATA_TO_ADDR);
-	jtag_dr_shift_16(p, 0xA500);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-
-	for (index = 0; index < length; index++) {
-		/* Set RW to write */
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x2408);
-
-		/* Set address */
-		jtag_ir_shift(p, IR_ADDR_16BIT);
-		jtag_dr_shift_16(p, address);
-
-		/* Set data */
-		jtag_ir_shift(p, IR_DATA_TO_ADDR);
-		jtag_dr_shift_16(p, data[index]);
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-
-		/* Set RW to read */
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x2409);
-
-		/* provide TCLKs
-		 * min. 33 for F149 and F449
-		 */
-		p->f->jtdev_tclk_strobe(p, 35);
-		address += 2;
-
-		if (p->failed)
-			break;
-	}
-
-	/* Set RW to write */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2408);
-
-	/* FCTL1 register */
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_16(p, 0x0128); // FIXME
-
-	/* Disable FLASH write */
-	jtag_ir_shift(p, IR_DATA_TO_ADDR);
-	jtag_dr_shift_16(p, 0xA500);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-
-	/* Reset FCTL3 */
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_16(p, 0x012C); // FIXME
-	jtag_ir_shift(p, IR_DATA_TO_ADDR);
-	jtag_dr_shift_16(p, 0xA510);
-	jtag_tclk_set(p);
-}
-
-/* Performs a mass erase (with and w/o info memory) or a segment erase of a
- * FLASH module specified by the given mode and address. Large memory devices
- * get additional mass erase operations to meet the spec.
- * erase_mode   : ERASE_MASS, ERASE_MAIN, ERASE_SGMT
- * erase_address: address within the selected segment
- */
-static void jlf16_erase_flash(struct jtdev *p, unsigned int erase_mode,
-		      address_t erase_address)
-{	// SLAU320AJ name: EraseFLASH
-	unsigned int number_of_strobes = 4820;	/* default for segment erase */
-	unsigned int loop_counter;
-	unsigned int max_loop_count = 1;	/* erase cycle repeating for mass erase */
-
-	dbg_printc("%04x: %04x\n", erase_mode, erase_address);
-
-	if ((erase_mode == JTAG_ERASE_MASS) || (erase_mode == JTAG_ERASE_MAIN)) {
-		number_of_strobes = 5300;	/* Larger Flash memories require */
-		max_loop_count = 19;		/* additional cycles for erase. */
-		erase_address = 0xfffe;		/* overwrite given address */
-	}
-
-	for (loop_counter = max_loop_count; loop_counter > 0; loop_counter--) {
-		jlf16_halt_cpu(p);
-		jtag_tclk_clr(p);
-
-		/* Set RW to write */
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x2408);
-
-		/* FCTL1 address */
-		jtag_ir_shift(p, IR_ADDR_16BIT);
-		jtag_dr_shift_16(p, 0x0128); // FIXME
-
-		/* Enable erase mode */
-		jtag_ir_shift(p, IR_DATA_TO_ADDR);
-		jtag_dr_shift_16(p, erase_mode); // FIXME?
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-
-		/* FCTL2 address */
-		jtag_ir_shift(p, IR_ADDR_16BIT);
-		jtag_dr_shift_16(p, 0x012A); // FIXME
-
-		/* MCLK is source, DIV=1 */
-		jtag_ir_shift(p, IR_DATA_TO_ADDR);
-		jtag_dr_shift_16(p, 0xA540);
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-
-		/* FCTL3 address */
-		jtag_ir_shift(p, IR_ADDR_16BIT);
-		jtag_dr_shift_16(p, 0x012C); // FIXME
-
-		/* Clear FCTL3 */
-		jtag_ir_shift(p, IR_DATA_TO_ADDR);
-		jtag_dr_shift_16(p, 0xA500);
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-
-		/* Set erase address */
-		jtag_ir_shift(p, IR_ADDR_16BIT);
-		jtag_dr_shift_16(p, erase_address);
-
-		/* Dummy write to start erase */
-		jtag_ir_shift(p, IR_DATA_TO_ADDR);
-		jtag_dr_shift_16(p, 0x55AA);
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-
-		/* Set RW to read */
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x2409);
-
-		/* provide TCLKs */
-		p->f->jtdev_tclk_strobe(p, number_of_strobes);
-
-		/* Set RW to write */
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x2408);
-
-		/* FCTL1 address */
-		jtag_ir_shift(p, IR_ADDR_16BIT);
-		jtag_dr_shift_16(p, 0x0128); // FIXME
-
-		/* Disable erase */
-		jtag_ir_shift(p, IR_DATA_TO_ADDR);
-		jtag_dr_shift_16(p, 0xA500);
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-
-		/* Reset FCTL3 */
-		jtag_ir_shift(p, IR_ADDR_16BIT);
-		jtag_dr_shift_16(p, 0x012C); // FIXME
-		jtag_ir_shift(p, IR_DATA_TO_ADDR);
-		jtag_dr_shift_16(p, 0xA510);
-		jtag_tclk_set(p);
-
-		jlf16_release_cpu(p);
-	}
-}
-
-/* Reads a register from the target CPU */
-static address_t jlf16_read_reg(struct jtdev *p, int reg)
-{	// libmsp430 BIOS name: ReadCpuReg
-	unsigned int value;
-
-	dbg_printc("%d\n", reg);
-
-	/* Set CPU into instruction fetch mode */
-	jlf16_set_instruction_fetch(p);
-
-	/* CPU controls RW & BYTE */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x3401);
-
-	jtag_ir_shift(p, IR_DATA_16BIT);
-
-	/* "jmp $-4" instruction */
-	/* PC - 4 -> PC          */
-	/* needs 2 clock cycles  */
-	jtag_dr_shift_16(p, 0x3ffd);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	/* "mov Rn,&0x01fe" instruction
-	 * Rn -> &0x01fe
-	 * PC is advanced 4 bytes by this instruction
-	 * needs 4 clock cycles
-	 * it's a ROM address, write has no effect, but
-	 * the registers value is placed on the databus
-	 */
-	jtag_dr_shift_16(p, 0x4082 | (((unsigned int)reg << 8) & 0x0f00) );
-	jtag_tclk_clr(p);
-	//jtag_ir_shift(p, IR_DATA_CAPTURE); // TODO: ???
-	jtag_tclk_set(p);
-	//jtag_ir_shift(p, IR_DATA_16BIT); // TODO: ???
-	jtag_dr_shift_16(p, 0x01fe);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	/* older code did an extra clock cycle -- don't do this! will put the
-	 * current instruction word on the data bus instead of the register value
-	 * on the G2452, making it useless. the clock cycles are still required to
-	 * move to the next instruction, but those should be done later. */
-	/*jtag_tclk_clr(p);
-	jtag_tclk_set(p);*/
-
-	/* Read databus which contains the registers value */
-	jtag_ir_shift(p, IR_DATA_CAPTURE);
-	value = jtag_dr_shift_16(p, 0x0000);
-
-	jtag_tclk_clr(p);
-
-	/* JTAG controls RW & BYTE */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2401);
-
-	jtag_tclk_set(p);
-
-	dbg_printc("%d -> %04x\n", reg, value);
-
-	/* Return value read from register */
-	return value;
-}
-
-/* Writes a value into a register of the target CPU */
-static void jlf16_write_reg(struct jtdev *p, int reg, address_t value)
-{	// SLAU320AJ name: SetPC
-	/* Set CPU into instruction fetch mode */
-	dbg_printc("%d <- %04x\n", reg, value);
-
-	jlf16_set_instruction_fetch(p);
-
-	/* CPU controls RW & BYTE */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x3401);
-
-	jtag_ir_shift(p, IR_DATA_16BIT);
-
-	/* "jmp $-4" instruction */
-	/* PC - 4 -> PC          */
-	/* needs 4 clock cycles  */
-	jtag_dr_shift_16(p, 0x3ffd);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	/* "mov #value,Rn" instruction
-	 * value -> Rn
-	 * PC is advanced 4 bytes by this instruction
-	 * needs 2 clock cycles
-	 */
-	jtag_dr_shift_16(p, 0x4030 | (reg & 0x000f) );
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_dr_shift_16(p, value);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	// TODO: ???
-	//jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	//jtag_tclk_clr(p);
-
-	/* JTAG controls RW & BYTE */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2401);
-}
-
-/*----------------------------------------------------------------------------*/
-static void jlf16_single_step( struct jtdev *p )
-{	// libmsp430 BIOS name: SingleStep
-	unsigned int loop_counter;
-
-	dbg_printc("\n");
-
-	jlf16_set_instruction_fetch(p);
-
-	/* CPU controls RW & BYTE */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x3401);
-
-	/* clock CPU until next instruction fetch cycle  */
-	/* failure after 10 clock cycles                 */
-	/* this is more than for the longest instruction */
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	for (loop_counter = 10; loop_counter > 0; loop_counter--) {
-		jtag_tclk_clr(p);
-		jtag_tclk_set(p);
-		if ((jtag_dr_shift_16(p, 0x0000) & 0x0080) == 0x0080) {
-			break;
-		}
-	}
-
-	/* JTAG controls RW & BYTE */
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x2401);
-
-	jlf16_set_instruction_fetch(p);
-
-	if (loop_counter == 0) {
-		/* timeout reached */
-		printc_err("jtaglib_cpu16: single step failed\n");
-		p->failed = 1;
-	}
-}
-
-/*----------------------------------------------------------------------------*/
-static unsigned int jlf16_set_breakpoint( struct jtdev *p,int bp_num, address_t bp_addr )
-{
-	/* The breakpoint logic is explained in 'SLAU414c EEM.pdf' */
-	/* A good overview is given with Figure 1-1                */
-	/* MBx           is TBx         in EEM_defs.h              */
-	/* CPU Stop      is BREAKREACT  in EEM_defs.h              */
-	/* State Storage is STOR_REACT  in EEM_defs.h              */
-	/* Cycle Counter is EVENT_REACT in EEM_defs.h              */
-
-	unsigned int breakreact;
-
-	if (bp_num >= 8) {
-		/* there are no more than 8 breakpoints in EEM */
-		printc_err("jlf16_set_breakpoint: failed setting "
-			   "breakpoint %d at %04x\n", bp_num, bp_addr);
-		p->failed = 1;
-		return 0;
-	}
-
-	dbg_printc("num=%d addr=%04x\n", bp_num, bp_addr);
-
-	if (bp_num < 0) {
-		/* disable all breakpoints by deleting the BREAKREACT
-		 * register */
-		jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE);
-		jtag_dr_shift_16(p, BREAKREACT + WRITE);
-		jtag_dr_shift_16(p, 0x0000);
-		return 1;
-	}
-
-	/* set breakpoint */
-	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE);
-	jtag_dr_shift_16(p, GENCTRL + WRITE);
-	jtag_dr_shift_16(p, EEM_EN + CLEAR_STOP + EMU_CLK_EN + EMU_FEAT_EN);
-
-	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
-	jtag_dr_shift_16(p, 8*bp_num + MBTRIGxVAL + WRITE);
-	jtag_dr_shift_16(p, bp_addr);
-
-	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
-	jtag_dr_shift_16(p, 8*bp_num + MBTRIGxCTL + WRITE);
-	jtag_dr_shift_16(p, MAB + TRIG_0 + CMP_EQUAL);
-
-	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
-	jtag_dr_shift_16(p, 8*bp_num + MBTRIGxMSK + WRITE);
-	jtag_dr_shift_16(p, NO_MASK);
-
-	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
-	jtag_dr_shift_16(p, 8*bp_num + MBTRIGxCMB + WRITE);
-	jtag_dr_shift_16(p, 1<<bp_num);
-
-	/* read the actual setting of the BREAKREACT register         */
-	/* while reading a 1 is automatically shifted into LSB        */
-	/* this will be undone and the bit for the new breakpoint set */
-	/* then the updated value is stored back                      */
-	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
-	breakreact  = jtag_dr_shift_16(p, BREAKREACT + READ);
-	breakreact += jtag_dr_shift_16(p, 0x000);
-	breakreact  = (breakreact >> 1) | (1 << bp_num);
-	jtag_dr_shift_16(p, BREAKREACT + WRITE);
-	jtag_dr_shift_16(p, breakreact);
-	return 1;
-}
-
-/*----------------------------------------------------------------------------*/
-static unsigned int jlf16_cpu_state( struct jtdev *p )
-{	// libmsp430 BIOS name: WaitForEem(?)
-	jtag_ir_shift(p, IR_EMEX_READ_CONTROL);
-
-	if ((jtag_dr_shift_16(p, 0x0000) & 0x0080) == 0x0080) {
-		dbg_printc("halted\n");
-		return 1; /* halted */
-	} else {
-		dbg_printc("running\n");
-		return 0; /* running */
-	}
-}
-
-/*----------------------------------------------------------------------------*/
-static int jlf16_get_config_fuses( struct jtdev *p )
-{	// always the same?
-	jtag_ir_shift(p, IR_CONFIG_FUSES);
-
-	return jtag_dr_shift_8(p, 0);
-}
-
-/* ------------------------------------------------------------------------- */
-
-const struct jtaglib_funcs jlf_cpu16 = {
-	.jlf_get_device = jlf16_get_device,
-
-	.jlf_read_mem = jlf16_read_mem,
-	.jlf_read_mem_quick = jlf16_read_mem_quick,
-	.jlf_write_mem = jlf16_write_mem,
-	.jlf_write_mem_quick = jlf16_write_mem_quick,
-
-	.jlf_execute_puc = jlf16_execute_puc,
-	.jlf_release_device = jlf16_release_device,
-
-	.jlf_verify_mem = jlf16_verify_mem,
-	.jlf_erase_check = jlf16_erase_check,
-
-	.jlf_write_flash = jlf16_write_flash,
-	.jlf_erase_flash = jlf16_erase_flash,
-
-	.jlf_context_save = jtag_dev_default_context_save,
-	.jlf_context_restore = jtag_dev_default_context_restore,
-	.jlf_regs_update = jtag_dev_default_regs_update,
-	.jlf_regs_flush = jtag_dev_default_regs_flush,
-
-	.jlf_read_reg = jlf16_read_reg,
-	.jlf_write_reg = jlf16_write_reg,
-	.jlf_single_step = jlf16_single_step,
-	.jlf_set_breakpoint = jlf16_set_breakpoint,
-	.jlf_cpu_state = jlf16_cpu_state,
-	.jlf_get_config_fuses = jlf16_get_config_fuses,
-};
-
--- a/drivers/jtaglib_cpux.c
+++ b/drivers/jtaglib_cpux.c
@ -1,7 +0,0 @@
-
-#include "jtaglib.h"
-#include "jtaglib_defs.h"
-#include "output.h"
-
-const struct jtaglib_funcs jlf_cpux;
-
--- a/drivers/jtaglib_cpuxv2.c
+++ b/drivers/jtaglib_cpuxv2.c
@ -1,889 +0,0 @@
-
-#include <stdbool.h>
-
-#include "jtaglib.h"
-#include "jtaglib_defs.h"
-#include "output.h"
-
-#define SAFE_FRAM_PC 0x0004
-
-#ifdef DEBUG_JTAGLIB
-#define dbg_printc(fmt, ...) printc_dbg("jlfxv2: %s:%d " fmt, __func__, __LINE__, ##__VA_ARGS__)
-#else
-#define dbg_printc(fmt, ...) do{}while(0)
-#endif
-
-static address_t jlfxv2_read_reg(struct jtdev *p, int reg);
-static void jlfxv2_write_reg(struct jtdev *p, int reg, address_t value);
-
-// FIXME:
-// * context save/restore:
-//   * pc gets written correctly on single-step
-//   * pc does NOT get written on 'run'
-//   * pc does NOT get read properly on run break
-// * implement flash write, erase, verify stuff
-// * check how ^ works in practice on flash & FRAM devices
-// * single step: not working!
-// * breakpoints are a big TODO
-
-static int jlfxv2_check_full_emu_state_ex(struct jtdev *p, const char* fnname)
-{	// see SLAU320AJ ExecutePOR
-	uint16_t dr;
-	uint8_t jtag_id;
-
-	for (int i = 0; i < 1/*10*/; ++i) {
-		jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-		if (((dr = jtag_dr_shift_16(p, 0)) & 0x0301) == 0x0301) {
-			return 1; // OK
-		}
-
-		printc_err("jlfxv2: %s: not in full emu state, while expected!"
-				" (dr=%04x jid=%02x)\n", fnname, dr, jtag_id);
-	}
-	p->failed = 1;
-
-	return 0;
-}
-#define jlfxv2_check_full_emu_state(p) jlfxv2_check_full_emu_state_ex((p), __func__)
-
-static int jlfxv2_wait_sync_ex(struct jtdev *p, const char* fnname) {
-	uint16_t dr;
-	uint8_t jtag_id;
-
-	jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	for (int i = 0; i < 50; ++i) {
-		if ((dr = jtag_dr_shift_16(p, 0)) & 0x0200)
-			return 1;
-	}
-
-	printc_err("jlfxv2: %s: couldn't sync! (dr=%04x jid=%02x)\n", fnname, dr, jtag_id);
-	return 0;
-}
-#define jlfxv2_wait_sync(p) jlfxv2_wait_sync_ex((p), __func__)
-
-
-static uint8_t bitswap_nyb(uint8_t in) {
-	return ((in >> 3) & 1) | ((in >> 1) & 2) | ((in << 1) & 4) | ((in << 3) & 8);
-}
-static uint8_t bitswap(uint8_t in) {
-	return (bitswap_nyb(in&0xf) << 4) | bitswap_nyb(in>>4);
-}
-static address_t demangle_mab(address_t mab) {
-	address_t page = mab & 0xf0000;
-	address_t bot  = mab & 0x0ff00;
-	address_t top  = mab & 0x000ff;
-
-	address_t ret = page | (bot >> 8) | ((address_t)bitswap(top) << 8);
-	//dbg_printc("demangle: %05x -> %05x\n", mab, ret);
-	return ret;
-}
-
-static int jlfxv2_set_pc(struct jtdev *p, address_t addr)
-{
-	address_t addr_rb;
-
-	dbg_printc("set pc %05x\n", addr);
-
-	if (!jlfxv2_wait_sync(p)) return -1;
-	/*jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	jtag_dr_shift_16(p, 0);*/
-	jtag_tclk_clr(p);
-
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_tclk_set(p);
-	jtag_dr_shift_16(p, 0x0080 | ((addr >> 8) & 0x0f00));
-	jtag_tclk_clr(p);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x1400);
-
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_dr_shift_16(p, addr & 0xffff);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	jtag_dr_shift_16(p, 0x4303);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	addr_rb = demangle_mab(jtag_dr_shift_20(p, 0));
-	dbg_printc("set pc MAB -> %05x%s\n", addr_rb,
-			(addr!=addr_rb)?", aieee!":"");
-
-	return (addr == addr_rb) ? 0 : -1;
-}
-/*static address_t jlfxv2_get_pc(struct jtdev *p)
-{
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	jtag_dr_shift_16(p, 0);
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	jtag_dr_shift_20(p, 0);
-
-	address_t addr = jtag_dr_shift_20(p, 0);
-
-	dbg_printc("get pc %05x\n", addr);
-
-	return addr;
-}*/
-
-/* Compares the computed PSA (Pseudo Signature Analysis) value to the PSA
- * value shifted out from the target device. It is used for very fast data
- * block write or erasure verification.
- * start_address: start of data
- * length       : number of data
- * data         : pointer to data, 0 for erase check
- * RETURN       : 1 - comparison was successful
- *                0 - otherwise
- */
-static int jlfxv2_verify_mem(struct jtdev *p,
-			   unsigned int start_address,
-			   unsigned int length,
-			   const uint16_t *data)
-{	// SLAU320AJ name: VerifyMem/VerifyPSA
-	uint16_t crc, psaval;
-
-	jtag_execute_puc(p);
-
-	crc = start_address - 2;
-	if (jlfxv2_set_pc(p, start_address) < 0) return -1;
-
-	jtag_tclk_set(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_dr_shift_16(p, start_address - 2);
-	jtag_ir_shift(p, IR_DATA_PSA);
-
-	for (unsigned int addr = 0; addr < length; ++addr) {
-		if (crc & 0x8000) {
-			crc ^= 0x0805;
-			crc <<= 1;
-			crc |= 1;
-		} else {
-			crc <<= 1;
-		}
-
-		if (data) crc ^= data[addr];
-		else crc ^= 0xffff;
-
-		jtag_tclk_clr(p);
-		/* Go through DR path without shifting data in/out */
-		jtag_tms_sequence(p, 6, 0x19); /* TMS=1 0 0 1 1 0 ; 6 clocks */
-		jtag_tclk_set(p);
-	}
-
-	jtag_ir_shift(p, IR_SHIFT_OUT_PSA);
-	psaval = jtag_dr_shift_16(p, 0);
-
-	jtag_execute_puc(p);
-
-	return (psaval == crc) ? 1 : 0;
-}
-
-/* ------------------------------------------------------------------------- */
-
-static int jlfxv2_erase_check(struct jtdev *p, unsigned int start_address,
-			   unsigned int length)
-{
-	return jlfxv2_verify_mem(p, start_address, length, NULL);
-}
-
-static unsigned int jlfxv2_get_device(struct jtdev *p)
-{	// SLAU320AJ name: GetDevice (and SyncJtag_AssertPor)
-	unsigned int jtag_id = 0;
-	int i;
-
-	dbg_printc("jlfxv2: get_device\n");
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x1501);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	for (i = 0; i < 50; ++i) {
-		if ((jtag_dr_shift_16(p, 0) & (1<<9)) == (1<<9))
-			break;
-	}
-
-	if (i == 50) {
-		printc_err("jlfxv2_get_device: failed\n");
-		p->failed = 1;
-		return 0;
-	}
-
-	jtag_id = jtag_execute_puc(p);
-	return jtag_id;
-}
-
-static void jlfxv2_read_mem_quick(struct jtdev *p, address_t address,
-			 unsigned int length, uint16_t *data);
-
-/* Reads one byte/word from a given address
- * format : 8-byte, 16-word
- * address: address of memory
- * return : content of memory
- */
-static uint16_t jlfxv2_read_mem(struct jtdev *p, unsigned int format, address_t address)
-{	// SLAU320AJ name: ReadMem
-	uint16_t r = 0;
-
-	dbg_printc("read mem %05x\n", address);
-	if (!jlfxv2_check_full_emu_state(p))
-		return 0;
-
-	// the code below (an attempt at implementing the real read_mem) doesn't
-	// work, so let's use read_mem_quick as a backup
-	jlfxv2_read_mem_quick(p, address ^ (address & 1), 1, &r);
-	if (format == 8) {
-		if (address & 1) return (r >> 8) & 0xff;
-		else return r & 0xff;
-	} else return r;
-
-	/*//jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	//uint16_t dr = jtag_dr_shift_16(p, 0);
-	jtag_tclk_clr(p);
-	//jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	//jtag_dr_shift_16(p, (format == 16) ? 0x0501 : 0x0511);
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_20(p, address ^ (address & 1));
-	//jtag_ir_shift(p, IR_DATA_TO_ADDR);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_DATA_CAPTURE);
-
-	r = jtag_dr_shift_16(p, 0);
-
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	if (format == 8) {
-		if (address & 1) return (r >> 8) & 0xff;
-		else return r & 0xff;
-	} else return r;*/
-}
-
-/* Reads an array of words from target memory
- * address: address to read from
- * length : number of word to read
- * data   : memory to write to
- */
-static void jlfxv2_read_mem_quick(struct jtdev *p, address_t address,
-			 unsigned int length, uint16_t *data)
-{	// SLAU320AJ name: ReadMemQuick
-	if (!jlfxv2_check_full_emu_state(p))
-		return;
-
-	dbg_printc("read mem quick %05x..%05x\n", address, address+length*2);
-	if (jlfxv2_set_pc(p, address) < 0) return;
-
-	//jtag_tclk_set(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-	jtag_tclk_set(p);
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	jtag_ir_shift(p, IR_DATA_QUICK);
-
-	for (unsigned int i = 0; i < length; ++i) {
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-		data[i] = jtag_dr_shift_16(p, 0);
-	}
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	jtag_dr_shift_16(p, 0);
-
-	jlfxv2_set_pc(p, SAFE_FRAM_PC);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-	jtag_tclk_set(p);
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-}
-
-static void jlfxv2_write_mem_quick(struct jtdev *p, address_t address,
-			  unsigned int length, const uint16_t *data);
-
-/* Writes one byte/word at a given address
- * format : 8-byte, 16-word
- * address: address to be written
- * data   : data to write
- */
-static void jlfxv2_write_mem(struct jtdev *p, unsigned int format,
-		    address_t address, uint16_t data)
-{	// SLAU320AJ name: WriteMem
-	dbg_printc("write mem: %d %06x <- %04x\n", format, address, data);
-
-	if (format == 8) {
-		p->failed = 1;
-		printc_err("jlfxv2 write mem: byte access not yet implemented!\n");
-		return;
-	}
-
-	// same story as with read_mem
-	jlfxv2_write_mem_quick(p, address, 1, &data);
-
-	if (!jlfxv2_check_full_emu_state(p))
-		return;
-
-	/*jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	uint16_t dr = jtag_dr_shift_16(p, 0);
-
-	dbg_printc("write mem %d: %06x<-%04x: dr=%04x\n", format, address, data, dr);
-
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, (format == 16) ? 0x0500 : 0x0510);
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_20(p, address);
-	jtag_tclk_set(p);
-	jtag_ir_shift(p, IR_DATA_TO_ADDR);
-	jtag_dr_shift_16(p, data);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);*/
-}
-
-/* Writes an array of words into target memory
- * address: address to write to
- * length : number of word to write
- * data   : data to write
- */
-static void jlfxv2_write_mem_quick(struct jtdev *p, address_t address,
-			  unsigned int length, const uint16_t *data)
-{	// SLAU320AJ name: WriteMemQuick
-	/*for (unsigned int i = 0; i < length; ++i) {
-		jlfxv2_write_mem(p, 16, address + i*2, data[i]);
-	}*/
-
-	dbg_printc("write mem quick: %05x..%05x\n", address, address+length*2);
-
-	if (!jlfxv2_check_full_emu_state(p))
-		return;
-
-	if (jlfxv2_set_pc(p, address) < 0) return;
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0500);
-	jtag_tclk_set(p);
-
-	jtag_ir_shift(p, IR_DATA_QUICK);
-
-
-	for (unsigned int i = 0; i < length; ++i) {
-		jtag_tclk_set(p);
-		jtag_dr_shift_16(p, data[i]);
-		jtag_tclk_clr(p);
-	}
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-	jtag_tclk_set(p);
-
-	jlfxv2_set_pc(p, SAFE_FRAM_PC);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-	jtag_tclk_set(p);
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-}
-
-/* Execute a Power-Up Clear (PUC) using JTAG CNTRL SIG register
- * return: JTAG ID
- */
-static unsigned int jlfxv2_execute_puc(struct jtdev *p)
-{	// SLAU320AJ name: ExecutePOR
-	unsigned int jtag_id;
-
-	dbg_printc("execute_puc\n");
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x1501);
-
-	if (!jlfxv2_wait_sync(p)) return -1;
-
-	jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-
-	// empty CPU pipeline
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-
-	/* Apply and remove reset */
-	jtag_dr_shift_16(p, 0x0C01);
-	delay_ms(40);
-	jtag_dr_shift_16(p, 0x0401);
-
-	if (jtag_id == 0x91 || jtag_id == 0X99 || jtag_id == 0x98) {
-		jtag_ir_shift(p, IR_DATA_16BIT);
-
-		jtag_tclk_clr(p);
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-		jtag_tclk_set(p);
-
-		jtag_dr_shift_16(p, SAFE_FRAM_PC);
-
-		jtag_tclk_clr(p);
-		jtag_tclk_set(p);
-
-		if (jtag_id == 0x91) {
-			jtag_tclk_clr(p);
-			jtag_tclk_set(p);
-		}
-
-		jtag_ir_shift(p, IR_DATA_CAPTURE);
-	} else { // TODO: this if jtag_id == 0x91, else other branch?
-		jtag_tclk_clr(p);
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-		jtag_tclk_set(p);
-		jtag_tclk_clr(p);
-		jtag_tclk_set(p);
-	}
-
-	// two more cycles to release CPU internal POR delay signals
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	return jtag_id;
-}
-
-/* Release the target device from JTAG control
- * address: 0xFFFE - perform Reset,
- *                   load Reset Vector into PC
- *          0xFFFF - start execution at current
- *                   PC position
- *          other  - load Address into PC
- */
-static void jlfxv2_release_device(struct jtdev *p, address_t address)
-{	// SLAU320AJ name: ReleaseDevice. from Replicator430 code, not in the PDF
-	switch (address) {
-	case 0xffff: // BOR
-		dbg_printc("jlfxv2: release device: BOR\n");
-
-		jtag_ir_shift(p, IR_TEST_REG);
-		jtag_dr_shift_16(p, 0x0200);
-		delay_ms(5);
-		break;
-	case 0xfffe: // reset
-		dbg_printc("jlfxv2: release device: SRST\n");
-
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x0C01);
-		delay_ms(40);
-		jtag_dr_shift_16(p, 0x0401);
-		jtag_ir_shift(p, IR_CNTRL_SIG_RELEASE);
-		break;
-	default:
-		dbg_printc("jlfxv2: release device: PC=%05x\n", address);
-
-		jlfxv2_set_pc(p, address);
-		jtag_tclk_set(p);
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x0401);
-		jtag_ir_shift(p, IR_ADDR_CAPTURE);
-
-		jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-		jtag_tclk_clr(p);
-		if (jtag_dr_shift_16(p, 0) & 2/*CNTRL_SIG_HALT*/) {
-			jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-			jtag_dr_shift_16(p, 0x0403);
-		}
-		jtag_tclk_set(p);
-
-		jtag_ir_shift(p, IR_CNTRL_SIG_RELEASE);
-		break;
-	}
-}
-
-/* Programs/verifies an array of words into a FLASH by using the
- * FLASH controller. The JTAG FLASH register isn't needed.
- * start_address: start in FLASH
- * length       : number of words
- * data         : pointer to data
- */
-static void jlfxv2_write_flash(struct jtdev *p, address_t start_address,
-		      unsigned int length, const uint16_t *data)
-{	// SLAU320AJ name: WriteFLASH
-	// TODO: implement!
-}
-
-/* Performs a mass erase (with and w/o info memory) or a segment erase of a
- * FLASH module specified by the given mode and address. Large memory devices
- * get additional mass erase operations to meet the spec.
- * erase_mode   : ERASE_MASS, ERASE_MAIN, ERASE_SGMT
- * erase_address: address within the selected segment
- */
-static void jlfxv2_erase_flash(struct jtdev *p, unsigned int erase_mode,
-		      address_t erase_address)
-{	// SLAU320AJ name: EraseFLASH
-	// TODO: implement!
-}
-
-/* Reads a register from the target CPU */
-static address_t jlfxv2_read_reg(struct jtdev *p, int reg)
-{	// libmsp430 BIOS name: ReadCpuReg
-	uint16_t reglo, reghi;
-	uint16_t jtag_id, jmb_addr;
-	const bool alt_addr = false;//true;
-
-	if (reg == 3) return 0; // CG
-
-	dbg_printc("read reg %d\n", reg);
-	if (!jlfxv2_check_full_emu_state(p))
-		return 0;
-
-	jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_tclk_set(p);
-	jtag_dr_shift_16(p, ((reg << 8) & 0x0f00) | 0x0060);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x1401);
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	if (alt_addr) {
-		jtag_dr_shift_16(p, 0x0ff6);
-	} else {
-		jmb_addr = (jtag_id == 0x98) ? 0x14c : 0x18c;
-		jtag_dr_shift_16(p, jmb_addr);
-	}
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_dr_shift_16(p, 0x3ffd);
-	jtag_tclk_clr(p);
-
-	if (alt_addr) {
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x0501);
-	}
-
-	jtag_ir_shift(p, IR_DATA_CAPTURE);
-	jtag_tclk_set(p);
-	reglo = jtag_dr_shift_16(p, 0);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	reghi = jtag_dr_shift_16(p, 0);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	if (!alt_addr) {
-		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-		jtag_dr_shift_16(p, 0x0501);
-	}
-
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_DATA_CAPTURE);
-	jtag_tclk_set(p);
-
-	dbg_printc("read reg %d: lo=%04x hi=%04x\n", reg, reglo, reghi);
-
-	jlfxv2_set_pc(p, SAFE_FRAM_PC);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-	jtag_tclk_set(p);
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-
-	return reglo | ((address_t)reghi << 16);
-}
-
-/* Writes a value into a register of the target CPU */
-static void jlfxv2_write_reg(struct jtdev *p, int reg, address_t value)
-{	// SLAU320AJ name: SetPC
-	if (reg == 0 || reg == 3) return; // pc, cg
-
-	dbg_printc("write reg %d %06x\n", reg, value);
-
-	if (!jlfxv2_check_full_emu_state(p)) return;
-
-	/*jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	jtag_dr_shift_16(p, 0);*/
-
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_tclk_set(p);
-	jtag_dr_shift_16(p, 0x0080 | ((value >> 8) & 0x0f00) | (reg & 0xf));
-
-	//jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x1401/*1400*/);
-	jtag_ir_shift(p, IR_DATA_16BIT);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_dr_shift_16(p, value & 0xffff);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-
-	jtag_dr_shift_16(p, 0x3ffd); // rewind PC
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	jtag_dr_shift_20(p, 0);
-	jtag_tclk_set(p);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-
-	jtag_ir_shift(p, IR_DATA_CAPTURE);
-	jtag_tclk_set(p);
-	//jtag_dr_shift_20(p, 0);
-}
-
-/*----------------------------------------------------------------------------*/
-static void jlfxv2_single_step( struct jtdev *p )
-{	// libmsp430 BIOS name: SingleStep
-
-	int i, timeout;
-	uint16_t tmp;
-	dbg_printc("single step\n");
-
-	// TODO: fix this. it only performs an instruction fetch but not much else
-
-	/*jtag_ir_shift(p, IR_EMEX_READ_CONTROL);
-	timeout = 3000;
-	for (i = 0; i < timeout; ++i)
-		if (jtag_dr_shift_16(p, 0) & EEM_STOPPED)
-			break;
-	if (i == timeout) {
-		printc_err("jlfxv2_single_step: EEM timeout\n");
-		goto err;
-	}*/
-
-	jtag_ir_shift(p, IR_EMEX_WRITE_CONTROL);
-	jtag_dr_shift_16(p, EMU_CLK_EN | EEM_EN);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x1501);
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	timeout = 50;
-	for (i = 0; i < timeout; ++i) {
-		tmp = jtag_dr_shift_16(p, 0);
-		if (tmp != 0xffff && (tmp & 0x200) == 0x0200)
-			break;
-	}
-	if (i == timeout) {
-		printc_err("jlfxv2_single_step: JTAG sync timeout\n");
-		goto err;
-	}
-
-	jtag_ir_shift(p, IR_EMEX_WRITE_CONTROL);
-	jtag_dr_shift_16(p, EMU_CLK_EN | CLEAR_STOP);
-	jtag_dr_shift_16(p, EMU_CLK_EN | CLEAR_STOP | EEM_EN);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x1501);
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	timeout = 30000;
-	for (i = 0; i < timeout; ++i) {
-		if ((jtag_dr_shift_16(p, 0) & 8) == 0) break;
-
-		jtag_tclk_clr(p);
-		jtag_tclk_set(p);
-
-		jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	}
-	if (i == timeout) {
-		printc_err("jlfxv2_single_step: single-step timeout\n");
-		goto err;
-	}
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	timeout = 10000;
-	for (i = 0; i < timeout; ++i) {
-		jtag_tclk_clr(p);
-		tmp = jtag_dr_shift_16(p, 0);
-		jtag_tclk_set(p);
-
-		if ((tmp & CNTRL_SIG_CPUSUSP) == CNTRL_SIG_CPUSUSP) break;
-	}
-	if (i == timeout) {
-		printc_err("jlfxv2_single_step: pipeline empty timeout\n");
-		goto err;
-	}
-
-	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
-	jtag_dr_shift_16(p, 0x0501);
-
-	return;
-err:
-	p->failed = 1;
-}
-
-/*----------------------------------------------------------------------------*/
-static unsigned int jlfxv2_set_breakpoint( struct jtdev *p,int bp_num, address_t bp_addr )
-{
-	/* The breakpoint logic is explained in 'SLAU414c EEM.pdf' */
-	/* A good overview is given with Figure 1-1                */
-	/* MBx           is TBx         in EEM_defs.h              */
-	/* CPU Stop      is BREAKREACT  in EEM_defs.h              */
-	/* State Storage is STOR_REACT  in EEM_defs.h              */
-	/* Cycle Counter is EVENT_REACT in EEM_defs.h              */
-
-	// TODO: implement
-
-	return 0;
-}
-
-/*----------------------------------------------------------------------------*/
-static unsigned int jlfxv2_cpu_state( struct jtdev *p )
-{	// libmsp430 BIOS name: WaitForEem(?)
-	jtag_ir_shift(p, IR_EMEX_READ_CONTROL);
-
-	if ((jtag_dr_shift_16(p, 0x0000) & EEM_STOPPED) == EEM_STOPPED) {
-		dbg_printc("cpu_state: halted\n");
-		return 1; /* halted */
-	} else {
-		dbg_printc("cpu_state: running\n");
-		return 0; /* running */
-	}
-}
-
-/*----------------------------------------------------------------------------*/
-static int jlfxv2_get_config_fuses( struct jtdev *p )
-{	// always the same?
-	jtag_ir_shift(p, IR_CONFIG_FUSES);
-
-	return jtag_dr_shift_8(p, 0);
-}
-
-/* ------------------------------------------------------------------------- */
-
-static void jlfxv2_context_save(struct jtdev *p, bool after_puc) {
-	address_t wdtctl_a = (p->jtag_id == 0x89) ? 0x0120 : 0x015c,
-	          rst_vec = 0x0fffe,
-	          mab, entrypt_addr;
-
-	dbg_printc("context save, %s\n", after_puc?"after PUC":"normal");
-
-	jtag_ir_shift(p, IR_EMEX_WRITE_CONTROL);
-	jtag_dr_shift_16(p, EMU_FEAT_EN | EMU_CLK_EN | CLEAR_STOP);
-	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
-	jtag_dr_shift_16(p, 0);
-
-	/* ReadMemWordXv2 */
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_20(p, wdtctl_a);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_DATA_CAPTURE);
-	p->wdtctl = jtag_dr_shift_16(p, 0) & 0xff;
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	dbg_printc("WDTCTL: %04x\n", p->wdtctl);
-	/* ReadMemWordXv2 */
-	/* actually not using the result for this, but adding this makes it work
-	 * somehow */
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_ADDR_16BIT);
-	jtag_dr_shift_20(p, rst_vec);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_DATA_CAPTURE);
-	entrypt_addr = jtag_dr_shift_16(p, 0);
-	jtag_tclk_set(p);
-	jtag_tclk_clr(p);
-	jtag_tclk_set(p);
-	dbg_printc("ENTRYPOINT: %04x\n", entrypt_addr);
-
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	mab = demangle_mab(jtag_dr_shift_20(p, 0));
-	dbg_printc("PC MAB: %05x\n", mab);
-	//mab = rst_vec + 4;
-
-	/* back up program counter */
-	if (after_puc) {
-		p->regs[0] = rst_vec; /* yeah.. */
-	} else {
-		p->regs[0] = mab - 4;
-	}
-
-	if (p->regs[0] == rst_vec) {
-		p->regs[0] = jtag_read_mem(p, 16, rst_vec);
-		dbg_printc("ENTRY TRY1: %04x\n", p->regs[0]);
-		if (p->regs[0] == 0xffff || p->regs[0] == 0x3fff) {
-			p->regs[0] = jtag_read_mem(p, 16, rst_vec);
-			dbg_printc("ENTRY TRY2: %04x\n", p->regs[0]);
-		}
-	}
-
-	/* disable watchdog */
-	//p->wdtctl = jtag_read_mem(p, 16, wdtctl_a) & 0xff;
-	dbg_printc("WDTCTL: %04x\n", p->wdtctl);
-	jtag_write_mem(p, 16, wdtctl_a, p->wdtctl | 0x5a80);
-
-	/* also back up stack pointer & status register */
-	p->regs[1] = jtag_read_reg(p, 1);
-	p->regs[2] = jtag_read_reg(p, 2);
-
-	const static uint16_t data[] = {0x3fff,0x3fff,0x3fff};
-	jtag_write_mem_quick(p, 0x00004, sizeof(data)/sizeof(*data), data);
-}
-static void jlfxv2_context_restore(struct jtdev *p) {
-	dbg_printc("context restore\n");
-
-	/* basically the inverse of context_save */
-	address_t wdtctl_a = (p->jtag_id == 0x89) ? 0x0120 : 0x015c;
-
-	jtag_write_reg(p, 1, p->regs[1]);
-	jtag_write_reg(p, 2, p->regs[2]);
-
-	jtag_write_mem(p, 16, wdtctl_a, p->wdtctl | 0x5a00);
-
-	jlfxv2_set_pc(p, p->regs[0]);
-}
-
-/* ------------------------------------------------------------------------- */
-
-const struct jtaglib_funcs jlf_cpuxv2 = {
-	.jlf_get_device = jlfxv2_get_device,
-
-	.jlf_read_mem = jlfxv2_read_mem,
-	.jlf_read_mem_quick = jlfxv2_read_mem_quick,
-	.jlf_write_mem = jlfxv2_write_mem,
-	.jlf_write_mem_quick = jlfxv2_write_mem_quick,
-
-	.jlf_execute_puc = jlfxv2_execute_puc,
-	.jlf_release_device = jlfxv2_release_device,
-
-	.jlf_verify_mem = jlfxv2_verify_mem,
-	.jlf_erase_check = jlfxv2_erase_check,
-
-	.jlf_write_flash = jlfxv2_write_flash,
-	.jlf_erase_flash = jlfxv2_erase_flash,
-
-	.jlf_context_save = jlfxv2_context_save,
-	.jlf_context_restore = jlfxv2_context_restore,
-	.jlf_regs_update = jtag_dev_default_regs_update,
-	.jlf_regs_flush = jtag_dev_default_regs_flush,
-
-	.jlf_read_reg = jlfxv2_read_reg,
-	.jlf_write_reg = jlfxv2_write_reg,
-	.jlf_single_step = jlfxv2_single_step,
-	.jlf_set_breakpoint = jlfxv2_set_breakpoint,
-	.jlf_cpu_state = jlfxv2_cpu_state,
-	.jlf_get_config_fuses = jlfxv2_get_config_fuses,
-};
-
--- a/drivers/jtaglib_defs.h
+++ b/drivers/jtaglib_defs.h
@ -1,77 +0,0 @@
-
-#include "jtaglib.h"
-#include "eem_defs.h"
-
-/* Flash erasing modes */
-#define JTAG_ERASE_MASS 0xA506
-#define JTAG_ERASE_MAIN 0xA504
-#define JTAG_ERASE_SGMT 0xA502
-
-#define JTAG_ID_IS_XV2(x) ((x)==0x91||(x)==0x95||(x)==0x98||(x)==0x99)
-
-/* Instructions for the JTAG control signal register in reverse bit order
- */
-#define IR_CNTRL_SIG_16BIT	0xC8	/* 0x13 */
-#define IR_CNTRL_SIG_CAPTURE	0x28	/* 0x14 */
-#define IR_CNTRL_SIG_RELEASE	0xA8	/* 0x15 */
-#define IR_COREIP_ID		0xE8	/* 0x17 */
-/* Instructions for the JTAG data register */
-#define IR_DATA_16BIT		0x82	/* 0x41 */
-#define IR_DATA_CAPTURE		0x42	/* 0x42 */
-#define IR_DATA_QUICK		0xC2	/* 0x43 */
-/* Instructions for the JTAG address register */
-#define IR_ADDR_16BIT		0xC1	/* 0x83 */
-#define IR_ADDR_CAPTURE		0x21	/* 0x84 */
-#define IR_DATA_TO_ADDR		0xA1	/* 0x85 */
-#define IR_DEVICE_ID		0xE1	/* 0x87 */
-/* Instructions for the JTAG PSA mode */
-#define IR_DATA_PSA		0x22	/* 0x44 */
-#define IR_SHIFT_OUT_PSA	0x62	/* 0x46 */
-/* Instructions for the JTAG Fuse */
-#define IR_PREPARE_BLOW		0x44	/* 0x22 */
-#define IR_EX_BLOW		0x24	/* 0x24 */
-/* Instructions for the JTAG mailbox */
-#define IR_TEST_REG		0x54	/* 0x2A */
-/* Instructions for the Configuration Fuse */
-#define IR_CONFIG_FUSES		0x94
-/* Bypass instruction */
-#define IR_BYPASS		0xFF	/* 0xFF */
-/* Instructions for the EEM */
-#define IR_EMEX_DATA_EXCHANGE	0x90 /* 0x09 */
-#define IR_EMEX_WRITE_CONTROL	0x30 /* 0x0C */
-#define IR_EMEX_READ_CONTROL	0xD0 /* 0x0B */
-
-/* EEM stuff */
-#define EEM_STOPPED 0x0080
-#define EEM_EN      0x0001
-#define CLEAR_STOP  0x0002
-#define EMU_CLK_EN  0x0004
-
-#define CNTRL_SIG_CPUSUSP (1<<8)
-
-#define jtag_tms_set(p)		p->f->jtdev_tms(p, 1)
-#define jtag_tms_clr(p)		p->f->jtdev_tms(p, 0)
-#define jtag_tck_set(p)		p->f->jtdev_tck(p, 1)
-#define jtag_tck_clr(p)		p->f->jtdev_tck(p, 0)
-#define jtag_tdi_set(p)		p->f->jtdev_tdi(p, 1)
-#define jtag_tdi_clr(p)		p->f->jtdev_tdi(p, 0)
-#define jtag_tclk_set(p)	p->f->jtdev_tclk(p, 1)
-#define jtag_tclk_clr(p)	p->f->jtdev_tclk(p, 0)
-#define jtag_rst_set(p)		p->f->jtdev_rst(p, 1)
-#define jtag_rst_clr(p)		p->f->jtdev_rst(p, 0)
-#define jtag_tst_set(p)		p->f->jtdev_tst(p, 1)
-#define jtag_tst_clr(p)		p->f->jtdev_tst(p, 0)
-
-#define jtag_led_green_on(p)	p->f->jtdev_led_green(p, 1)
-#define jtag_led_green_off(p)	p->f->jtdev_led_green(p, 0)
-#define jtag_led_red_on(p)	p->f->jtdev_led_red(p, 1)
-#define jtag_led_red_off(p)	p->f->jtdev_led_red(p, 0)
-
-#define jtag_ir_shift(p, ir) p->f->jtdev_ir_shift(p, ir)
-#define jtag_dr_shift_8(p, dr) p->f->jtdev_dr_shift_8(p, dr)
-#define jtag_dr_shift_16(p, dr) p->f->jtdev_dr_shift_16(p, dr)
-#define jtag_dr_shift_20(p, dr) p->f->jtdev_dr_shift_20(p, dr)
-#define jtag_tms_sequence(p, bits, tms) p->f->jtdev_tms_sequence(p, bits, tms)
-#define jtag_init_dap(p) p->f->jtdev_init_dap(p)
-
-
--- a/drivers/jtdev.c
+++ b/drivers/jtdev.c
@ -359,7 +359,6 @@ const struct jtdev_func jtdev_func_pif = {
  .jtdev_ir_shift    = jtag_default_ir_shift,
  .jtdev_dr_shift_8  = jtag_default_dr_shift_8,
  .jtdev_dr_shift_16 = jtag_default_dr_shift_16,
-  .jtdev_dr_shift_20 = jtag_default_dr_shift_20,
  .jtdev_tms_sequence= jtag_default_tms_sequence,
  .jtdev_init_dap    = jtag_default_init_dap
 };
--- a/drivers/jtdev.h
+++ b/drivers/jtdev.h
@ -22,25 +22,16 @@

 #include <stdint.h>

-#include "device.h"
-#include "jtaglib_defs.h"
-
 struct jtdev_func;
 struct jtdev {
-	struct device base;
 	int		port;
 	uint8_t		data_register;
 	uint8_t		control_register;
-	uint8_t		jtag_id;
-	uint8_t		cpu_type;
 	int		failed;
 	const struct jtdev_func * f;
-
-	address_t regs[DEVICE_NUM_REGS];
-	uint16_t wdtctl;
 };

-struct jtdev_func {
+struct jtdev_func{
 /* Initialize/destroy a parallel-port JTAG interface. jtdev_open()
 * returns 0 on success or -1 if an error occurs.
 *
@ -77,7 +68,6 @@ struct jtdev_func {
 	uint8_t (*jtdev_ir_shift)(struct jtdev *p, uint8_t ir);
 	uint8_t (*jtdev_dr_shift_8)(struct jtdev *p, uint8_t dr);
 	uint16_t (*jtdev_dr_shift_16)(struct jtdev *p, uint16_t dr);
-	uint32_t (*jtdev_dr_shift_20)(struct jtdev *p, uint32_t dr);
 	void (*jtdev_tms_sequence)(struct jtdev *p, int bits, unsigned int value);
 	void (*jtdev_init_dap)(struct jtdev *p);
 };
--- a/drivers/jtdev_bus_pirate.c
+++ b/drivers/jtdev_bus_pirate.c
@ -357,7 +357,6 @@ const struct jtdev_func jtdev_func_bp = {
  .jtdev_ir_shift    = jtag_default_ir_shift,
  .jtdev_dr_shift_8  = jtag_default_dr_shift_8,
  .jtdev_dr_shift_16 = jtag_default_dr_shift_16,
-  .jtdev_dr_shift_20 = jtag_default_dr_shift_20,
  .jtdev_tms_sequence= jtag_default_tms_sequence,
  .jtdev_init_dap    = jtag_default_init_dap
 };
--- a/drivers/jtdev_gpio.c
+++ b/drivers/jtdev_gpio.c
@ -268,7 +268,6 @@ const struct jtdev_func jtdev_func_gpio = {
  .jtdev_ir_shift    = jtag_default_ir_shift,
  .jtdev_dr_shift_8  = jtag_default_dr_shift_8,
  .jtdev_dr_shift_16 = jtag_default_dr_shift_16,
-  .jtdev_dr_shift_20 = jtag_default_dr_shift_20,
  .jtdev_tms_sequence= jtag_default_tms_sequence,
  .jtdev_init_dap    = jtag_default_init_dap
 };
--- a/drivers/mehfet.c
+++ b/drivers/mehfet.c
@ -32,6 +32,7 @@


 struct mehfet_device {
+	struct device base;
 	struct jtdev jtag;
 	transport_t trans;
 	enum mehfet_conn connstat;
@ -156,17 +157,6 @@ static uint16_t jtmf_dr_shift_16(struct jtdev *p, uint16_t dr)

 	return outbuf[0] | ((uint16_t)outbuf[1] << 8);
 }
-static uint32_t jtmf_dr_shift_20(struct jtdev *p, uint32_t dr)
-{
-	struct mehfet_device* d = JTMF_GET_DEV(p);
-
-	uint8_t inbuf[3] = { dr & 0xff, (dr >> 8) & 0xff, (dr >> 16) & 0x0f };
-	uint8_t outbuf[3];
-	int r = mehfet_cmd_drshift(d->trans, 20, inbuf, outbuf);
-	if (r < 0) p->failed = 1;
-
-	return outbuf[0] | ((uint32_t)outbuf[1] << 8) | ((uint32_t)outbuf[2] << 16);
-}
 static void jtmf_tms_sequence(struct jtdev *p, int bits, unsigned int value)
 {
 	struct mehfet_device* d = JTMF_GET_DEV(p);
@ -217,12 +207,262 @@ static const struct jtdev_func jtdev_func_mehfet = {
 	.jtdev_ir_shift    = jtmf_ir_shift,
 	.jtdev_dr_shift_8  = jtmf_dr_shift_8,
 	.jtdev_dr_shift_16 = jtmf_dr_shift_16,
-	.jtdev_dr_shift_20 = jtmf_dr_shift_20,
 	.jtdev_tms_sequence= jtmf_tms_sequence,
 	.jtdev_init_dap    = jtmf_init_dap
 };


+/*---------------------------------------------------------------------------*/
+
+// TODO: these five are kinda copied from pif.c, should be deduplicated
+
+static int read_words(device_t dev_base, const struct chipinfo_memory *m,
+		address_t addr, address_t len, uint8_t *data)
+{
+#ifdef DEBUG_MEHFET_DRIVER
+	printc_dbg("mehfet: read_words: addr=0x%04x, len=0x%x\n", addr, len);
+#endif
+
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+	struct jtdev *p = &dev->jtag;
+
+	for (unsigned int index = 0; index < len; index += 2) {
+		unsigned int word = jtag_read_mem(p, 16, addr+index);
+		data[index  ] =  word       & 0x00ff;
+		data[index+1] = (word >> 8) & 0x00ff;
+	}
+
+	return p->failed ? -1 : len;
+}
+
+static int write_ram_word(struct jtdev *p, address_t addr, uint16_t value)
+{
+	jtag_write_mem(p, 16, addr, value);
+
+	return p->failed ? -1 : 0;
+}
+
+static int write_flash_block(struct jtdev *p, address_t addr,
+		address_t len, const uint8_t *data)
+{
+	uint16_t* word = malloc( len / 2 * sizeof(*word) );
+	if (!word) {
+		pr_error("mehfet: failed to allocate memory");
+		return -1;
+	}
+
+	for (unsigned int i = 0; i < len/2; i++) {
+		word[i]=data[2*i] + (((uint16_t)data[2*i+1]) << 8);
+	}
+	jtag_write_flash(p, addr, len/2, word);
+
+	free(word);
+
+	return p->failed ? -1 : 0;
+}
+
+/* Write a word-aligned block to any kind of memory.
+ * returns the number of bytes written or -1 on failure
+ */
+static int write_words(device_t dev_base, const struct chipinfo_memory *m,
+		address_t addr, address_t len, const uint8_t *data)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+	struct jtdev *p = &dev->jtag;
+	int r;
+
+	if (m->type != CHIPINFO_MEMTYPE_FLASH) {
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet: write_words: addr=0x%04x, len=0x%x data=0x%04x\n",
+				addr, len, r16le(data));
+		if (len != 2) {
+			printc_dbg("mehfet: WARN: write_words: len != 2! but 0x%04x\n", len);
+			__builtin_trap();
+		}
+#endif
+		len = 2;
+		r = write_ram_word(p, addr, r16le(data));
+	} else {
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet: write_flash_block: addr=0x%04x, len=0x%x\n", addr, len);
+#endif
+		r = write_flash_block(p, addr, len, data);
+	}
+
+	if (r < 0) {
+		printc_err("mehfet: write_words at address 0x%x failed\n", addr);
+		return -1;
+	}
+
+	return len;
+}
+
+/*---------------------------------------------------------------------------*/
+
+static int mehfet_readmem(device_t dev_base, address_t addr,
+			   uint8_t *mem, address_t len)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+	dev->jtag.failed = 0;
+	return readmem(dev_base, addr, mem, len, read_words);
+}
+
+static int mehfet_writemem(device_t dev_base, address_t addr,
+			    const uint8_t *mem, address_t len)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+	dev->jtag.failed = 0;
+	return writemem(dev_base, addr, mem, len, write_words, read_words);
+}
+
+static int mehfet_setregs(device_t dev_base, const address_t *regs)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+
+	dev->jtag.failed = 0;
+
+#ifdef DEBUG_MEHFET_DRIVER
+	printc_dbg("mehfet: set regs\n");
+#endif
+	for (int i = 0; i < DEVICE_NUM_REGS; i++) {
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet:  [%d] = 0x%04x\n", i, regs[i]);
+#endif
+		jtag_write_reg(&dev->jtag, i, regs[i]);
+	}
+	return dev->jtag.failed ? -1 : 0;
+}
+
+static int mehfet_getregs(device_t dev_base, address_t *regs)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+
+	dev->jtag.failed = 0;
+
+#ifdef DEBUG_MEHFET_DRIVER
+	printc_dbg("mehfet: get regs\n");
+#endif
+	for (int i = 0; i < DEVICE_NUM_REGS; i++) {
+		regs[i] = jtag_read_reg(&dev->jtag, i);
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet:  [%d] = 0x%04x\n", i, regs[i]);
+#endif
+	}
+
+	return dev->jtag.failed ? -1 : 0;
+}
+
+static int mehfet_ctl(device_t dev_base, device_ctl_t type)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+
+	dev->jtag.failed = 0;
+
+	switch (type) {
+	case DEVICE_CTL_RESET:
+		/* perform soft reset */
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet: soft reset (PUC)\n");
+#endif
+		jtag_execute_puc(&dev->jtag);
+		break;
+
+	case DEVICE_CTL_RUN:
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet: set breakpoints\n");
+#endif
+		/* transfer changed breakpoints to device */
+		if (jtag_refresh_bps("mehfet", &dev->base, &dev->jtag) < 0) return -1;
+
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet: run @ current PC\n");
+#endif
+		/* start program execution at current PC */
+		jtag_release_device(&dev->jtag, 0xffff);
+		break;
+
+	case DEVICE_CTL_HALT:
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet: halt\n");
+#endif
+		/* take device under JTAG control */
+		jtag_get_device(&dev->jtag);
+		break;
+
+	case DEVICE_CTL_STEP:
+#ifdef DEBUG_MEHFET_DRIVER
+		printc_dbg("mehfet: single-step\n");
+#endif
+		/* execute next instruction at current PC */
+		jtag_single_step(&dev->jtag);
+		break;
+
+	default:
+		printc_err("mehfet: unsupported operation %d\n", type);
+		return -1;
+	}
+
+	return dev->jtag.failed ? -1 : 0;
+}
+
+static device_status_t mehfet_poll(device_t dev_base)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+
+	if (delay_ms(100) < 0 || ctrlc_check())
+		return DEVICE_STATUS_INTR;
+
+	int r = jtag_cpu_state(&dev->jtag);
+
+#ifdef DEBUG_MEHFET_DRIVER
+	printc_dbg("mehfet: cpu state: %d\n", r);
+#endif
+
+	if (r == 1) return DEVICE_STATUS_HALTED;
+
+	return DEVICE_STATUS_RUNNING;
+}
+
+static int mehfet_erase(device_t dev_base, device_erase_type_t type,
+			 address_t addr)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+
+	dev->jtag.failed = 0;
+
+	switch (type) {
+	case DEVICE_ERASE_MAIN:
+		jtag_erase_flash(&dev->jtag, JTAG_ERASE_MAIN, addr);
+		break;
+	case DEVICE_ERASE_ALL:
+		jtag_erase_flash(&dev->jtag, JTAG_ERASE_MASS, addr);
+		break;
+	case DEVICE_ERASE_SEGMENT:
+		jtag_erase_flash(&dev->jtag, JTAG_ERASE_SGMT, addr);
+		break;
+	default: return -1;
+	}
+
+#ifdef DEBUG_MEHFET_DRIVER
+	printc_dbg("mehfet: erase flash %d at %04x: %s\n", type, addr, dev->jtag.failed ? "failed" : "succeeded");
+#endif
+
+	return dev->jtag.failed ? -1 : 0;
+}
+
+static int mehfet_getconfigfuses(device_t dev_base)
+{
+	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
+
+	int r = jtag_get_config_fuses(&dev->jtag);
+
+#ifdef DEBUG_MEHFET_DRIVER
+	printc_dbg("mehfet: get_config_fuses: %d\n", r);
+#endif
+
+	return r;
+}
+
 /*---------------------------------------------------------------------------*/

 static int check_dev_ok(struct mehfet_device* dev, const struct device_args* args,
@ -278,6 +518,32 @@ static int check_dev_ok(struct mehfet_device* dev, const struct device_args* arg
 	return 0;
 }

+static int init_device(struct mehfet_device* dev) {
+	printc_dbg("Starting JTAG\n");
+
+	unsigned int jtagid = jtag_init(&dev->jtag);
+	if (dev->jtag.failed) return -1;
+
+	printc("JTAG ID: 0x%02x\n", jtagid);
+	if (jtagid != 0x89 && jtagid != 0x91) {
+		printc_err("mehfet: unexpected JTAG ID: 0x%02x\n", jtagid);
+		jtag_release_device(&dev->jtag, 0xfffe);
+		return -1;
+	}
+
+	// JTAG fuse check has been performed, so we can now switch to a
+	// higher-speed physical transport suitable for ~350 kHz TCLK strobes used
+	// in (and required for) flash programming
+	int r = mehfet_cmd_set_clkspeed(dev->trans, true);
+	if (r < 0) {
+		jtag_release_device(&dev->jtag, 0xfffe);
+		return -1;
+	}
+
+	return 0;
+}
+
+
 static void mehfet_destroy(device_t dev_base) {
 	struct mehfet_device* dev = (struct mehfet_device*)dev_base;

@ -314,9 +580,9 @@ static device_t mehfet_open(const struct device_args* args) {
 		return NULL;
 	}

-	dev->jtag.base.type = &device_mehfet;
-	dev->jtag.base.max_breakpoints = 2; // TODO
-	dev->jtag.base.need_probe = 1; // TODO
+	dev->base.type = &device_mehfet;
+	dev->base.max_breakpoints = 2; // TODO
+	dev->base.need_probe = 1; // TODO
 	dev->jtag.f = &jtdev_func_mehfet;

 	// the MehFET currently doesn't have a designated PID, so we can't really
@ -342,13 +608,7 @@ static device_t mehfet_open(const struct device_args* args) {
 		goto FAIL;
 	}

-	r = jtag_dev_init(&dev->jtag);
-	if (r < 0) goto FAIL;
-
-	// JTAG fuse check has been performed, so we can now switch to a
-	// higher-speed physical transport suitable for ~350 kHz TCLK strobes used
-	// in (and required for) flash programming
-	r = mehfet_cmd_set_clkspeed(dev->trans, true);
+	r = init_device(dev);
 	if (r < 0) goto FAIL;

 #ifdef DEBUG_MEHFET_DRIVER
@ -367,13 +627,13 @@ const struct device_class device_mehfet = {
 	.help = "MehFET USB JTAG/SBW device",
 	.open     = mehfet_open,
 	.destroy  = mehfet_destroy,
-	.readmem  = jtag_dev_readmem,
-	.writemem = jtag_dev_writemem,
-	.getregs  = jtag_dev_getregs,
-	.setregs  = jtag_dev_setregs,
-	.ctl      = jtag_dev_ctl,
-	.poll     = jtag_dev_poll,
-	.erase    = jtag_dev_erase,
-	.getconfigfuses = jtag_dev_getconfigfuses
+	.readmem  = mehfet_readmem,
+	.writemem = mehfet_writemem,
+	.getregs  = mehfet_getregs,
+	.setregs  = mehfet_setregs,
+	.ctl      = mehfet_ctl,
+	.poll     = mehfet_poll,
+	.erase    = mehfet_erase,
+	.getconfigfuses = mehfet_getconfigfuses
 };

--- a/drivers/mehfet_proto.c
+++ b/drivers/mehfet_proto.c
@ -402,72 +402,6 @@ static uint8_t bitswap(uint8_t in) {
 	return (bitswap_nyb(in&0xf) << 4) | bitswap_nyb(in>>4);
 }

-#ifdef DEBUG_MEHFET_PROTO_DRIVER
-static const char* get_ir_name(uint8_t ir) {
-	switch (ir) {
-	case 0x09: return "EMEX_DATA_EXCHANGE";
-	case 0x0a: return "EMEX_READ_TRIGGER";
-	case 0x0b: return "EMEX_READ_CONTROL";
-	case 0x0c: return "EMEX_WRITE_CONTROL";
-	case 0x0d: return "EMEX_READ_CONTROL_2";
-	case 0x0e: return "EMEX_WRITE_CONTROL_2";
-
-	case 0x11: return "CNTRL_SIG_HIGH_BYTE";
-	case 0x12: return "CNTRL_SIG_LOW_BYTE";
-	case 0x13: return "CNTRL_SIG_16BIT";
-	case 0x14: return "CNTRL_SIG_CAPTURE";
-	case 0x15: return "CNTRL_SIG_RELEASE";
-	case 0x17: return "COREIP_ID";
-
-	case 0x19: return "FLASH_16BIT_UPDATE";
-	case 0x1a: return "FLASH_CAPTURE";
-	case 0x1b: return "FLASH_16BIT_IN";
-	case 0x1c: return "FLASH_UPDATE";
-
-	case 0x21: return "CNTRL";
-	case 0x22: return "PREPARE_BLOW";
-	case 0x24: return "EX_BLOX";
-	case 0x29: return "CONFIG_FUSES";
-	case 0x2a: return "TEST_REG";
-	case 0x2f: return "TEST_3V_REG";
-
-	case 0x31: return "DUAL_8BIT";
-	case 0x32: return "DUAL_CAPTURE";
-	case 0x33: return "SELECT_MAIN";
-	case 0x34: return "SELECT_ESP";
-
-	case 0x41: return "DATA_16BIT";
-	case 0x42: return "DATA_CAPTURE";
-	case 0x43: return "DATA_QUICK";
-	case 0x44: return "DATA_PSA";
-	case 0x45: return "DATA_16BIT_OPT";
-	case 0x46: return "SHIFT_OUT_PSA";
-	case 0x47: return "DTA";
-
-	case 0x59: return "ACCEPT_KEY";
-
-	case 0x61: return "JMB_EXCHANGE";
-	case 0x62: return "JSTATE_ID";
-	case 0x64: return "TDO_EVENT";
-	case 0x65: return "TDO_EVENT_CTL";
-
-	case 0x81: return "ADDR_HIGH_BYTE";
-	case 0x82: return "ADDR_LOW_BYTE";
-	case 0x83: return "ADDR_16BIT";
-	case 0x84: return "ADDR_CAPTURE";
-
-	case 0x85: return "DATA_TO_ADDR";
-	case 0x86: return "CAPTURE_CPU_REG";
-	case 0x87: return "DEVICE_ID";
-	case 0x88: return "JMB_WRITE_32BIT_MODE";
-
-	case 0xFF: return "BYPASS";
-
-	default: return NULL;
-	}
-}
-#endif
-
 int mehfet_cmd_irshift(transport_t t, uint8_t newir, uint8_t* oldir) {
 	if (!oldir) return -1;

@ -497,12 +431,7 @@ int mehfet_cmd_irshift(transport_t t, uint8_t newir, uint8_t* oldir) {
 	*oldir = buf[0];

 #ifdef DEBUG_MEHFET_PROTO_DRIVER
-	const char* s = get_ir_name(newir);
-	if (s) {
-		printc_dbg("mehfet: IRshift(new=IR_%s) = 0x%02x\n", s, *oldir);
-	} else {
-		printc_dbg("mehfet: IRshift(new=0x%02x) = 0x%02x\n", newir, *oldir);
-	}
+	printc_dbg("mehfet: IRshift(new=0x%02x) = 0x%02x\n", newir, *oldir);
 #endif

 	return 0;
@ -534,26 +463,9 @@ int mehfet_cmd_drshift(transport_t t, uint32_t nbits, const uint8_t* newdr, uint
 	memcpy(olddr, buf, nbytes);

 #ifdef DEBUG_MEHFET_PROTO_DRIVER
-	if (nbits == 16) {
-		uint16_t in, out;
-		in = newdr[0] | ((uint16_t)newdr[1] << 8);
-		out= olddr[0] | ((uint16_t)olddr[1] << 8);
-		printc_dbg("mehfet: DRshift(nbits=%u): in: %04x, out: %04x\n", nbits, in, out);
-	} else if (nbits == 20) {
-		uint32_t in, out;
-		in = newdr[0] | ((uint32_t)newdr[1] << 8) | ((uint32_t)newdr[2] << 16);
-		out= olddr[0] | ((uint32_t)olddr[1] << 8) | ((uint32_t)olddr[2] << 16);
-		printc_dbg("mehfet: DRshift(nbits=%u): in: %05x, out: %05x\n", nbits, in, out);
-	} else if (nbits == 32) {
-		uint32_t in, out;
-		in = newdr[0] | ((uint32_t)newdr[1] << 8) | ((uint32_t)newdr[2] << 16) | ((uint32_t)newdr[3] << 24);
-		out= olddr[0] | ((uint32_t)olddr[1] << 8) | ((uint32_t)olddr[2] << 16) | ((uint32_t)olddr[3] << 24);
-		printc_dbg("mehfet: DRshift(nbits=%u): in: %08x, out: %08x\n", nbits, in, out);
-	} else {
-		printc_dbg("mehfet: DRshift(nbits=%u):\n", nbits);
-		debug_hexdump("\tin ", newdr, nbytes);
-		debug_hexdump("\tout", olddr, nbytes);
-	}
+	printc_dbg("mehfet: DRshift(nbits=%u):\n", nbits);
+	debug_hexdump("\tin ", newdr, nbytes);
+	debug_hexdump("\tout", olddr, nbytes);
 #endif

 	return 0;
--- a/drivers/pif.c
+++ b/drivers/pif.c
@ -35,10 +35,256 @@
 #include "ctrlc.h"

 struct pif_device {
+  struct device		base;
  struct jtdev		jtag;
 };

+/*============================================================================*/
+/* pif MSP430 JTAG operations */
+
 /*----------------------------------------------------------------------------*/
+/* Read a word-aligned block from any kind of memory
+ * returns the number of bytes read or -1 on failure
+ */
+static int read_words(device_t dev, const struct chipinfo_memory *m,
+		address_t addr, address_t len, uint8_t *data)
+{
+  struct pif_device *pif = (struct pif_device *)dev;
+  struct jtdev *p = &pif->jtag;
+  unsigned int index;
+  unsigned int word;
+
+  for ( index = 0; index < len; index += 2 ) {
+    word = jtag_read_mem( p, 16, addr+index );
+    data[index]   =  word       & 0x00ff;
+    data[index+1] = (word >> 8) & 0x00ff;
+  }
+
+  return p->failed ? -1 : len;
+}
+
+/*----------------------------------------------------------------------------*/
+/* Write a word to RAM */
+static int write_ram_word( struct jtdev *p, address_t addr,
+		    uint16_t  value )
+{
+  jtag_write_mem( p, 16, addr, value );
+
+  return p->failed ? -1 : 0;
+}
+
+/*----------------------------------------------------------------------------*/
+/* Write a word-aligned flash block. */
+/* The starting address must be within the flash memory range. */
+
+static int write_flash_block( struct jtdev *p, address_t addr,
+			      address_t len,
+			      const uint8_t *data)
+{
+  unsigned int i;
+  uint16_t *word;
+
+  word = malloc( len / 2 * sizeof(*word) );
+  if (!word) {
+	pr_error("pif: failed to allocate memory");
+	return -1;
+  }
+
+  for(i = 0; i < len/2; i++) {
+    word[i]=data[2*i] + (((uint16_t)data[2*i+1]) << 8);
+  }
+  jtag_write_flash( p, addr, len/2, word );
+
+  free(word);
+
+  return p->failed ? -1 : 0;
+}
+
+/* Write a word-aligned block to any kind of memory.
+ * returns the number of bytes written or -1 on failure
+ */
+static int write_words(device_t dev, const struct chipinfo_memory *m,
+		address_t addr, address_t len, const uint8_t *data)
+{
+  struct pif_device *pif = (struct pif_device *)dev;
+  struct jtdev *p = &pif->jtag;
+  int r;
+
+  if (m->type != CHIPINFO_MEMTYPE_FLASH) {
+    len = 2;
+    r = write_ram_word(p, addr, r16le(data));
+  } else {
+    r = write_flash_block(p, addr, len, data);
+  }
+
+  if (r < 0) {
+    printc_err("pif: write_words at address 0x%x failed\n", addr);
+    return -1;
+  }
+
+  return len;
+}
+
+/*----------------------------------------------------------------------------*/
+static int init_device(struct jtdev *p)
+{
+  unsigned int jtag_id;
+
+  printc_dbg("Starting JTAG\n");
+  jtag_id = jtag_init(p);
+  printc("JTAG ID: 0x%02x\n", jtag_id);
+  if (jtag_id != 0x89 && jtag_id != 0x91) {
+    printc_err("pif: unexpected JTAG ID: 0x%02x\n", jtag_id);
+    jtag_release_device(p, 0xfffe);
+    return -1;
+  }
+
+  return 0;
+}
+
+/*===== MSPDebug Device interface ============================================*/
+
+/*----------------------------------------------------------------------------*/
+static int pif_readmem( device_t  dev_base,
+			address_t addr,
+			uint8_t*  mem,
+			address_t len )
+{
+  struct pif_device *dev = (struct pif_device *)dev_base;
+  dev->jtag.failed = 0;
+  return readmem(dev_base, addr, mem, len, read_words);
+}
+
+/*----------------------------------------------------------------------------*/
+static int pif_writemem( device_t       dev_base,
+			 address_t      addr,
+			 const uint8_t* mem,
+			 address_t      len )
+{
+  struct pif_device *dev = (struct pif_device *)dev_base;
+  dev->jtag.failed = 0;
+  return writemem(dev_base, addr, mem, len, write_words, read_words);
+}
+
+/*----------------------------------------------------------------------------*/
+static int pif_getregs(device_t dev_base, address_t *regs)
+{
+  struct pif_device *dev = (struct pif_device *)dev_base;
+  int i;
+
+  dev->jtag.failed = 0;
+
+  for (i = 0; i < DEVICE_NUM_REGS; i++)
+    regs[i] = jtag_read_reg(&dev->jtag, i);
+
+  return dev->jtag.failed ? -1 : 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int pif_setregs( device_t dev_base, const address_t* regs )
+{
+  struct pif_device *dev = (struct pif_device *)dev_base;
+  int i;
+
+  dev->jtag.failed = 0;
+
+  for (i = 0; i < DEVICE_NUM_REGS; i++) {
+    jtag_write_reg( &dev->jtag, i, regs[i] );
+  }
+  return dev->jtag.failed ? -1 : 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int pif_ctl(device_t dev_base, device_ctl_t type)
+{
+  struct pif_device *dev = (struct pif_device *)dev_base;
+
+  dev->jtag.failed = 0;
+
+  switch (type) {
+    case DEVICE_CTL_RESET:
+      /* perform soft reset */
+      jtag_execute_puc(&dev->jtag);
+      break;
+
+    case DEVICE_CTL_RUN:
+      /* transfer changed breakpoints to device */
+      if (jtag_refresh_bps("pif", &dev->base, &dev->jtag) < 0) {
+	return -1;
+      }
+      /* start program execution at current PC */
+      jtag_release_device(&dev->jtag, 0xffff);
+      break;
+
+    case DEVICE_CTL_HALT:
+      /* take device under JTAG control */
+      jtag_get_device(&dev->jtag);
+      break;
+
+    case DEVICE_CTL_STEP:
+      /* execute next instruction at current PC */
+      jtag_single_step(&dev->jtag);
+      break;
+
+    default:
+      printc_err("pif: unsupported operation\n");
+      return -1;
+  }
+
+  return dev->jtag.failed ? -1 : 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static device_status_t pif_poll(device_t dev_base)
+{
+  struct pif_device *dev = (struct pif_device *)dev_base;
+
+  if (delay_ms(100) < 0 || ctrlc_check())
+    return DEVICE_STATUS_INTR;
+
+  if (jtag_cpu_state(&dev->jtag) == 1) {
+    return DEVICE_STATUS_HALTED;
+  }
+
+  return DEVICE_STATUS_RUNNING;
+}
+
+/*----------------------------------------------------------------------------*/
+static int pif_erase( device_t dev_base,
+		      device_erase_type_t type,
+		      address_t addr )
+{
+  struct pif_device *dev = (struct pif_device *)dev_base;
+
+  dev->jtag.failed = 0;
+
+  switch(type) {
+    case DEVICE_ERASE_MAIN:
+      jtag_erase_flash ( &dev->jtag, JTAG_ERASE_MAIN, addr );
+      break;
+    case DEVICE_ERASE_ALL:
+      jtag_erase_flash ( &dev->jtag, JTAG_ERASE_MASS, addr );
+      break;
+    case DEVICE_ERASE_SEGMENT:
+      jtag_erase_flash ( &dev->jtag, JTAG_ERASE_SGMT, addr );
+      break;
+    default:
+      return -1;
+  }
+
+  return dev->jtag.failed ? -1 : 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int pif_getconfigfuses(device_t dev_base)
+{
+  struct pif_device *dev = (struct pif_device *)dev_base;
+
+  return jtag_get_config_fuses(&dev->jtag);
+}
+
+/*----------------------------------------------------------------------------*/
+

 static device_t pif_open(const struct device_args *args)
 {
@ -61,9 +307,9 @@ static device_t pif_open(const struct device_args *args)
  }

  memset(dev, 0, sizeof(*dev));
-  dev->jtag.base.type = &device_pif;
-  dev->jtag.base.max_breakpoints = 2; //supported by all devices
-  dev->jtag.base.need_probe = 1;
+  dev->base.type = &device_pif;
+  dev->base.max_breakpoints = 2; //supported by all devices
+  dev->base.need_probe = 1;
  (&dev->jtag)->f = &jtdev_func_pif;

  if ((&dev->jtag)->f->jtdev_open(&dev->jtag, args->path) < 0) {
@ -72,13 +318,13 @@ static device_t pif_open(const struct device_args *args)
    return NULL;
  }

-  if (jtag_dev_init(&dev->jtag) < 0) {
+  if (init_device(&dev->jtag) < 0) {
    printc_err("pif: initialization failed\n");
    free(dev);
    return NULL;
  }

-  return &dev->jtag.base;
+  return &dev->base;
 }

 /*----------------------------------------------------------------------------*/
@ -105,8 +351,8 @@ static device_t gpio_open(const struct device_args *args)
  }

  memset(dev, 0, sizeof(*dev));
-  dev->jtag.base.type = &device_pif;
-  dev->jtag.base.max_breakpoints = 0;
+  dev->base.type = &device_pif;
+  dev->base.max_breakpoints = 0;
  (&dev->jtag)->f = &jtdev_func_gpio;

  if ((&dev->jtag)->f->jtdev_open(&dev->jtag, args->path) < 0) {
@ -115,13 +361,13 @@ static device_t gpio_open(const struct device_args *args)
    return NULL;
  }

-  if (jtag_dev_init(&dev->jtag) < 0) {
+  if (init_device(&dev->jtag) < 0) {
    printc_err("gpio: initialization failed\n");
    free(dev);
    return NULL;
  }

-  return &dev->jtag.base;
+  return &dev->base;
 }

 /*----------------------------------------------------------------------------*/
@ -148,9 +394,9 @@ static device_t bp_open(const struct device_args *args)
  }

  memset(dev, 0, sizeof(*dev));
-  dev->jtag.base.type = &device_pif;
-  dev->jtag.base.max_breakpoints = 2; //supported by all devices
-  dev->jtag.base.need_probe = 1;
+  dev->base.type = &device_pif;
+  dev->base.max_breakpoints = 2; //supported by all devices
+  dev->base.need_probe = 1;
  (&dev->jtag)->f = &jtdev_func_bp;

  if ((&dev->jtag)->f->jtdev_open(&dev->jtag, args->path) < 0) {
@ -159,13 +405,13 @@ static device_t bp_open(const struct device_args *args)
    return NULL;
  }

-  if (jtag_dev_init(&dev->jtag) < 0) {
+  if (init_device(&dev->jtag) < 0) {
    printc_err("bp: initialization failed\n");
    free(dev);
    return NULL;
  }

-  return &dev->jtag.base;
+  return &dev->base;
 }

 /*----------------------------------------------------------------------------*/
@ -186,14 +432,14 @@ const struct device_class device_pif = {
  .help     = "Parallel Port JTAG",
  .open     = pif_open,
  .destroy  = pif_destroy,
-  .readmem  = jtag_dev_readmem,
-  .writemem = jtag_dev_writemem,
-  .getregs  = jtag_dev_getregs,
-  .setregs  = jtag_dev_setregs,
-  .ctl      = jtag_dev_ctl,
-  .poll     = jtag_dev_poll,
-  .erase    = jtag_dev_erase,
-  .getconfigfuses = jtag_dev_getconfigfuses
+  .readmem  = pif_readmem,
+  .writemem = pif_writemem,
+  .getregs  = pif_getregs,
+  .setregs  = pif_setregs,
+  .ctl      = pif_ctl,
+  .poll     = pif_poll,
+  .erase    = pif_erase,
+  .getconfigfuses = pif_getconfigfuses
 };

 const struct device_class device_gpio = {
@ -201,14 +447,14 @@ const struct device_class device_gpio = {
  .help     = "/sys/class/gpio direct connect",
  .open     = gpio_open,
  .destroy  = pif_destroy,
-  .readmem  = jtag_dev_readmem,
-  .writemem = jtag_dev_writemem,
-  .getregs  = jtag_dev_getregs,
-  .setregs  = jtag_dev_setregs,
-  .ctl      = jtag_dev_ctl,
-  .poll     = jtag_dev_poll,
-  .erase    = jtag_dev_erase,
-  .getconfigfuses = jtag_dev_getconfigfuses
+  .readmem  = pif_readmem,
+  .writemem = pif_writemem,
+  .getregs  = pif_getregs,
+  .setregs  = pif_setregs,
+  .ctl      = pif_ctl,
+  .poll     = pif_poll,
+  .erase    = pif_erase,
+  .getconfigfuses = pif_getconfigfuses
 };

 const struct device_class device_bp = {
@ -216,12 +462,12 @@ const struct device_class device_bp = {
  .help     = "Bus Pirate JTAG, MISO-TDO, MOSI-TDI, CS-TMS, AUX-RESET, CLK-TCK",
  .open     = bp_open,
  .destroy  = pif_destroy,
-  .readmem  = jtag_dev_readmem,
-  .writemem = jtag_dev_writemem,
-  .getregs  = jtag_dev_getregs,
-  .setregs  = jtag_dev_setregs,
-  .ctl      = jtag_dev_ctl,
-  .poll     = jtag_dev_poll,
-  .erase    = jtag_dev_erase,
-  .getconfigfuses = jtag_dev_getconfigfuses
+  .readmem  = pif_readmem,
+  .writemem = pif_writemem,
+  .getregs  = pif_getregs,
+  .setregs  = pif_setregs,
+  .ctl      = pif_ctl,
+  .poll     = pif_poll,
+  .erase    = pif_erase,
+  .getconfigfuses = pif_getconfigfuses
 };
--- a/drivers/v3hil.c
+++ b/drivers/v3hil.c
@ -18,6 +18,7 @@

 #include <stdbool.h>
 #include <string.h>
+#include <stdio.h>
 #include "bytes.h"
 #include "v3hil.h"
 #include "dis.h"
@ -25,7 +26,7 @@
 #include "opdb.h"

 #ifdef DEBUG_V3HIL
-#define dbg_printc(fmt, ...) printc_dbg("v3hil: %s:%d: " fmt, __func__, __LINE__, ##__VA_ARGS__)
+#define dbg_printc(fmt, ...) printc_dbg("v3hil: " fmt, ##__VA_ARGS__)
 #else
 #define dbg_printc(fmt, ...) do{}while(0)
 #endif
@ -48,7 +49,7 @@ typedef enum {
 	HAL_PROTO_FID_SET_CHAIN_CONFIGURATION	= 0x0e,
 	HAL_PROTO_FID_GET_NUM_DEVICES		= 0x0f,
 	HAL_PROTO_FID_GET_INTERFACE_MODE	= 0x10,
-	HAL_PROTO_FID_GET_DEVICE_ID_PTR		= 0x11,
+	HAL_PROTO_FID_GET_DEVICE_ID_PTR = 0x11,
 	HAL_PROTO_FID_SJ_ASSERT_POR_SC		,
 	HAL_PROTO_FID_SJ_CONDITIONAL_SC		,
 	HAL_PROTO_FID_RC_RELEASE_JTAG		,
@ -63,7 +64,7 @@ typedef enum {
 	HAL_PROTO_FID_READ_ALL_CPU_REGS		,
 	HAL_PROTO_FID_WRITE_ALL_CPU_REGS	,
 	HAL_PROTO_FID_PSA			,
-	HAL_PROTO_FID_EXECUTE_FUNCLET		, /* 0x20 */
+	HAL_PROTO_FID_EXECUTE_FUNCLET		, // 0x20
 	HAL_PROTO_FID_EXECUTE_FUNCLET_JTAG	,
 	HAL_PROTO_FID_GET_DCO_FREQUENCY		,
 	HAL_PROTO_FID_GET_DCO_FREQUENCY_JTAG	,
@ -81,7 +82,7 @@ typedef enum {
 	HAL_PROTO_FID_EEM_DX_X			,
 	HAL_PROTO_FID_SINGLE_STEP_X		,
 	HAL_PROTO_FID_READ_ALL_CPU_REGS_X	,
-	HAL_PROTO_FID_WRITE_ALL_CPU_REGS_X	, /* 0x30 */
+	HAL_PROTO_FID_WRITE_ALL_CPU_REGS_X	, // 0x30
 	HAL_PROTO_FID_PSA_X			,
 	HAL_PROTO_FID_EXECUTE_FUNCLET_X		,
 	HAL_PROTO_FID_GET_DCO_FREQUENCY_X	,
@ -96,7 +97,7 @@ typedef enum {
 	HAL_PROTO_FID_READ_MEM_QUICK_XV2	,
 	HAL_PROTO_FID_WRITE_MEM_WORDS_XV2	,
 	HAL_PROTO_FID_EEM_DX_XV2		,
-	HAL_PROTO_FID_SINGLE_STEP_XV2		, /* 0x40 */
+	HAL_PROTO_FID_SINGLE_STEP_XV2		, // 0x40
 	HAL_PROTO_FID_READ_ALL_CPU_REGS_XV2	,
 	HAL_PROTO_FID_WRITE_ALL_CPU_REGS_XV2	,
 	HAL_PROTO_FID_PSA_XV2			,
@ -113,34 +114,34 @@ typedef enum {
 	HAL_PROTO_FID_SEND_JTAG_MAILBOX_XV2	,
 	HAL_PROTO_FID_SINGLE_STEP_JSTATE_XV2	,
 	HAL_PROTO_FID_POLL_JSTATE_REG_ET8	,
-	HAL_PROTO_FID_RESET_STATIC_GLOBAL_VARS	, /* 0x50 */
+	HAL_PROTO_FID_RESET_STATIC_GLOBAL_VARS	, // 0x50
 	HAL_PROTO_FID_RESET_430I		,
 	HAL_PROTO_FID_POLL_JSTATE_REG_430I	,
-	HAL_PROTO_FID_POLL_JSTATE_REG_20	,
-	HAL_PROTO_FID_SWITCH_MOSFET		,
-	HAL_PROTO_FID_RESET_L092		,
-	HAL_PROTO_FID_DUMMY_MACRO		,
-	HAL_PROTO_FID_RESET_5438XV2		,
-	HAL_PROTO_FID_LEA_SYNC_COND		,
-	HAL_PROTO_FID_GET_JTAG_ID_CODE_ARM	,
-	HAL_PROTO_FID_SCAN_AP_ARM		,
-	HAL_PROTO_FID_MEM_AP_TRANSACTION_ARM	,
-	HAL_PROTO_FID_READ_ALL_CPU_REGS_ARM	,
-	HAL_PROTO_FID_WRITE_ALL_CPU_REGS_ARM	,
-	HAL_PROTO_FID_ENABLE_DEBUG_ARM		,
-	HAL_PROTO_FID_DISABLE_DEBUG_ARM		,
-	HAL_PROTO_FID_RUN_ARM			, /* 0x60 */
-	HAL_PROTO_FID_HALT_ARM			,
-	HAL_PROTO_FID_RESET_ARM			,
-	HAL_PROTO_FID_SINGLE_STEP_ARM		,
-	HAL_PROTO_FID_WAIT_FOR_DEBUG_HALT_ARM	,
+	HAL_PROTO_FID_POLL_JSTATE_REG_20 ,
+	HAL_PROTO_FID_SWITCH_MOSFET ,
+	HAL_PROTO_FID_RESET_L092 ,
+	HAL_PROTO_FID_DUMMY_MACRO,
+	HAL_PROTO_FID_RESET_5438XV2,
+	HAL_PROTO_FID_LEA_SYNC_COND,
+	HAL_PROTO_FID_GET_JTAG_ID_CODE_ARM,
+	HAL_PROTO_FID_SCAN_AP_ARM,
+	HAL_PROTO_FID_MEM_AP_TRANSACTION_ARM,
+	HAL_PROTO_FID_READ_ALL_CPU_REGS_ARM,
+	HAL_PROTO_FID_WRITE_ALL_CPU_REGS_ARM,
+	HAL_PROTO_FID_ENABLE_DEBUG_ARM,
+	HAL_PROTO_FID_DISABLE_DEBUG_ARM,
+	HAL_PROTO_FID_RUN_ARM,
+	HAL_PROTO_FID_HALT_ARM,
+	HAL_PROTO_FID_RESET_ARM,
+	HAL_PROTO_FID_SINGLE_STEP_ARM,
+	HAL_PROTO_FID_WAIT_FOR_DEBUG_HALT_ARM,
 	HAL_PROTO_FID_MEM_AP_TRANSACTION_ARM_SWD,
-	HAL_PROTO_FID_GET_ITF_MODE_ARM		,
-	HAL_PROTO_FID_POLL_DSTATE_PCREG_ET	,
-	HAL_PROTO_FID_GET_CPU_ID_ARM		,
-	HAL_PROTO_FID_CHECK_DAP_LOCK_ARM	,
-	HAL_PROTO_FID_UNLOCK_DAP		,
-	HAL_PROTO_FID_USS_SYNC_COND		, /* 0x6b */
+	HAL_PROTO_FID_GET_ITF_MODE_ARM,
+	HAL_PROTO_FID_POLL_DSTATE_PCREG_ET,
+	HAL_PROTO_FID_GET_CPU_ID_ARM,
+	HAL_PROTO_FID_CHECK_DAP_LOCK_ARM,
+	HAL_PROTO_FID_UNLOCK_DAP,
+	HAL_PROTO_FID_USS_SYNC_COND
 } hal_proto_fid_t;

 /* Argument types for HAL_PROTO_FID_CONFIGURE */
@ -160,18 +161,17 @@ typedef enum {
 	HAL_PROTO_CONFIG_NO_BSL				= 0x0d,
 	HAL_PROTO_CONFIG_ALT_ROM_ADDR_FOR_CPU_READ	= 0x0e,
 	HAL_PROTO_CONFIG_ASSERT_BSL_VALID_BIT		= 0x0f,
-	HAL_PROTO_CONFIG_POWER_TESTREG_DEFAULT		= 0x10,
-	HAL_PROTO_CONFIG_POWER_TESTREGV3_DEFAULT	= 0x11,
-	HAL_PROTO_CONFIG_WDT_ADDRESS_5XX		= 0x12,
-	HAL_PROTO_CONFIG_SCS_BASE_ADDRESS		= 0x13,
-	HAL_PROTO_CONFIG_FPB_BASE_ADDRESS		= 0x14,
-	HAL_PROTO_CONFIG_INTERRUPT_OPTIONS		= 0x15,
-	HAL_PROTO_CONFIG_ULP_MSP432			= 0x16,
-	HAL_PROTO_CONFIG_JTAG_LOCK_5XX			= 0x17,
+	HAL_PROTO_CONFIG_POWER_TESTREG_DEFAULT = 0x10,
+	HAL_PROTO_CONFIG_POWER_TESTREGV3_DEFAULT = 0x11,
+	HAL_PROTO_CONFIG_WDT_ADDRESS_5XX = 0x12,
+	HAL_PROTO_CONFIG_SCS_BASE_ADDRESS = 0x13,
+	HAL_PROTO_CONFIG_FPB_BASE_ADDRESS = 0x14,
+	HAL_PROTO_CONFIG_INTERRUPT_OPTIONS = 0x15,
+	HAL_PROTO_CONFIG_ULP_MSP432 = 0x16,
+	HAL_PROTO_CONFIG_JTAG_LOCK_5XX = 0x17
 } hal_proto_config_t;

-static hal_proto_fid_t map_ver(const struct v3hil *h, hal_proto_fid_t src)
-{
+static hal_proto_fid_t map_ver(const struct v3hil *h, hal_proto_fid_t src) {
 	hal_proto_fid_t dst;

 	if (h->proto_ver < 0x0300 && src > HAL_PROTO_FID_GET_DEVICE_ID_PTR) {
@ -183,17 +183,24 @@ static hal_proto_fid_t map_ver(const struct v3hil *h, hal_proto_fid_t src)
 	dbg_printc("map ver: %02x -> %02x\n", src, dst);
 	return dst;
 }
-
 static hal_proto_fid_t map_fid(const struct v3hil *h, hal_proto_fid_t src)
 {
-	hal_proto_fid_t dst = h->chip->v3_functions[src];
+	hal_proto_fid_t src2 = src, dst, dst2;

-	if (dst == 0) {
-		dst = src;
+	if (src > HAL_PROTO_FID_GET_DEVICE_ID_PTR && false) {
+		src2 = src - 1;
 	}

-	dbg_printc("map fid: %02x -> %02x\n", src, dst);
-	return map_ver(h, dst);
+	dst = h->chip->v3_functions[src2];
+
+	if (dst) {
+		dst2 = dst;
+	} else {
+		dst2 = src;
+	}
+
+	dbg_printc("map fid: %02x -> %02x\n", src, dst2);
+	return map_ver(h, dst2);
 }

 void v3hil_init(struct v3hil *h, transport_t trans,
@ -303,8 +310,8 @@ int v3hil_start_jtag(struct v3hil *h, v3hil_jtag_type_t type)
 	}

 	printc_dbg("Device count: %d\n", h->hal.payload[0]);
-	return hal_proto_execute(&h->hal,
-			map_ver(h, HAL_PROTO_FID_SET_DEVICE_CHAIN_INFO), chain_id, 2);
+	return hal_proto_execute(&h->hal, map_ver(h, HAL_PROTO_FID_SET_DEVICE_CHAIN_INFO),
+				 chain_id, 2);
 }

 int v3hil_stop_jtag(struct v3hil *h)
@ -326,7 +333,7 @@ int v3hil_stop_jtag(struct v3hil *h)

 int v3hil_sync(struct v3hil *h)
 {
-	uint8_t data[32], datalen = 21;
+	uint8_t data[64], datalen = 21;

 	h->cal.is_cal = 0;

@ -364,7 +371,7 @@ int v3hil_sync(struct v3hil *h)
 	/* We can't use map_fid() because h->chip might be NULL -- this
 	 * function will be called before identification is complete.
 	 */
-	hal_proto_fid_t cmdid = (h->jtag_id == 0x89/*0x90*/)
+	hal_proto_fid_t cmdid = (h->jtag_id == 0x90)
 		? HAL_PROTO_FID_SJ_ASSERT_POR_SC
 		: HAL_PROTO_FID_SJ_ASSERT_POR_SC_XV2;

@ -391,7 +398,7 @@ int v3hil_sync(struct v3hil *h)
 	printc_dbg("v3hil: POR result: (len %d) ", h->hal.length);
 	for (int i = 0; i < h->hal.length; ++i) {
 		printc_dbg("%02x%s", h->hal.payload[i],
-			(i == h->hal.length - 1) ? "\n" : " ");
+				(i == h->hal.length - 1) ? "\n" : " ");
 	}
 #endif

@ -718,7 +725,7 @@ static int write_ram(struct v3hil *h, const struct chipinfo_memory *m,

 	dbg_printc("write ram\n");
 	hal_proto_fid_t fid = (m->bits == 8) ? HAL_PROTO_FID_WRITE_MEM_BYTES
-			: HAL_PROTO_FID_WRITE_MEM_WORDS;
+				  : HAL_PROTO_FID_WRITE_MEM_WORDS;
 	if (fram) fid = HAL_PROTO_FID_WRITE_FRAM_QUICK_XV2;
 	if (hal_proto_execute(&h->hal, map_fid(h, fid), data, size + 8) < 0) {
 		printc_err("v3hil: failed writing %d bytes to 0x%05x\n",
@ -744,11 +751,11 @@ int v3hil_write(struct v3hil *h, address_t addr,
 		size = 128;

 	if (m->type == CHIPINFO_MEMTYPE_FLASH) {
-		dbg_printc("write: call write flash\n");
+		dbg_printc("call write flash\n");
 		return write_flash(h, addr, mem, size);
 	}

-	dbg_printc("write: call write ram\n");
+	dbg_printc("call write ram\n");
 	return write_ram(h, m, addr, mem, size);
 }

@ -773,7 +780,7 @@ static int call_erase(struct v3hil *h,
 	w16le(data + 20, h->cal.cal1);
 	w32le(data + 22, 0xdeadbeef);

-	dbg_printc("erase: call funclet\n");
+	dbg_printc("call erase funclet\n");
 	if (hal_proto_execute(&h->hal,
 			map_fid(h, HAL_PROTO_FID_EXECUTE_FUNCLET),
 			data, 26) < 0) {
@ -807,10 +814,10 @@ int v3hil_erase(struct v3hil *h, address_t segment)
 	if (!flash)
 		printc_err("v3hil: can't find appropriate flash region\n");

-	dbg_printc("erase: calibrate\n");
+	dbg_printc("calibrate\n");
 	if (calibrate(h) < 0)
 		return -1;
-	dbg_printc("erase: upload funclet\n");
+	dbg_printc("upload erase funclet\n");
 	if (upload_funclet(h, ram, f) < 0)
 		return -1;

@ -821,13 +828,11 @@ int v3hil_erase(struct v3hil *h, address_t segment)
 		if (flash->banks)
 			bank_size /= flash->banks;

-		for (i = flash->banks; i >= 0; i--) {
+		for (i = flash->banks; i >= 0; i--)
 			dbg_printc("Erase bank %d\n", i);
-
 			if (call_erase(h, ram, f,
 				flash->offset + i * bank_size - 2, 0xa502) < 0)
 				return -1;
-		}
 	} else {
 		segment &= ~(flash->seg_size - 1);
 		segment |= flash->seg_size - 2;
@ -841,11 +846,11 @@ int v3hil_erase(struct v3hil *h, address_t segment)

 int v3hil_update_regs(struct v3hil *h)
 {
-	const hal_proto_fid_t fid = map_fid(h, HAL_PROTO_FID_READ_ALL_CPU_REGS);
+	const hal_proto_fid_t fid =
+		map_fid(h, HAL_PROTO_FID_READ_ALL_CPU_REGS);
 	const int reg_size = (fid == HAL_PROTO_FID_READ_ALL_CPU_REGS
-							|| fid == HAL_PROTO_FID_READ_ALL_CPU_REGS - 1)
-						? 2 : 3;
-
+		 				|| fid == HAL_PROTO_FID_READ_ALL_CPU_REGS - 1)
+				? 2 : 3;
 	int i;
 	int sptr = 0;

@ -881,10 +886,11 @@ int v3hil_update_regs(struct v3hil *h)

 int v3hil_flush_regs(struct v3hil *h)
 {
-	const hal_proto_fid_t fid = map_fid(h, HAL_PROTO_FID_WRITE_ALL_CPU_REGS);
-	const int reg_size = (fid == HAL_PROTO_FID_WRITE_ALL_CPU_REGS
-							|| fid == HAL_PROTO_FID_WRITE_ALL_CPU_REGS - 1)
-						? 2 : 3;
+	const hal_proto_fid_t fid =
+		map_fid(h, HAL_PROTO_FID_WRITE_ALL_CPU_REGS);
+	const int reg_size = (fid == HAL_PROTO_FID_READ_ALL_CPU_REGS
+		 				|| fid == HAL_PROTO_FID_READ_ALL_CPU_REGS - 1)
+				? 2 : 3;
 	int i;
 	int dptr = 0;
 	uint8_t data[64];
@ -1018,7 +1024,6 @@ static int set_param(struct v3hil *fet, hal_proto_config_t cfg,
 	}

 	data[0] = cfg;
-
 	dbg_printc("Set param 0x%02x to 0x%08x\n", cfg, value);
 	if (hal_proto_execute(&fet->hal, map_ver(fet, HAL_PROTO_FID_CONFIGURE),
 			      data, 8) < 0) {
@ -1121,11 +1126,8 @@ static int idproc_9x(struct v3hil *fet, uint32_t dev_id_ptr,
 		if (tag == 0xff)
 			break;

-		if ((tag == 0x14) && (len >= 2)) {
-			// FIXME: this looks like it's reading from the wrong address?
+		if ((tag == 0x14) && (len >= 2))
 			id->ver_sub_id = r16le(fet->hal.payload);
-			// FIXME: break here?
-		}

 		i += len;
 	}
@ -1145,10 +1147,12 @@ int v3hil_identify(struct v3hil *fet)
 			      NULL, 0) < 0)
 		return -1;

-	printc_dbg("ID:");
+#ifdef DEBUG_V3HIL
+	printc_dbg("v3hil: ID:");
 	for (i = 0; i < fet->hal.length; i++)
 		printc_dbg(" %02x", fet->hal.payload[i]);
 	printc_dbg("\n");
+#endif

 	if (fet->hal.length < 12) {
 		if (fet->hal.length == 2) {
@ -1172,12 +1176,11 @@ int v3hil_identify(struct v3hil *fet)
 			} else {
 				dev_id_ptr = r32le(fet->hal.payload + 0);

-				if (dev_id_ptr == 0) {
-					/* welp sometimes it's this instead (JTAG ID == 0x89?) */
-					dev_id_ptr = r32le(fet->hal.payload + 4);
+				if (dev_id_ptr == 0) { // welp sometimes it's this instead (JTAG ID == 0x89?)
+					dev_id_ptr = r32le(fet->hal.payload + 0);
 				}

-				id_data_addr = dev_id_ptr; /* idk */
+				id_data_addr = dev_id_ptr; // idk
 			}
 		} else {
 			printc_err("v3hil: short reply: %d\n", fet->hal.length);
@ -1203,12 +1206,13 @@ int v3hil_identify(struct v3hil *fet)
 	    set_param(fet, HAL_PROTO_CONFIG_POWER_TESTREG3V_MASK, 0) < 0 ||
 	    set_param(fet, HAL_PROTO_CONFIG_ALT_ROM_ADDR_FOR_CPU_READ, 0) < 0)
 		return -1;
-	set_param(fet, HAL_PROTO_CONFIG_NO_BSL, 0); /* is allowed to fail */
+	set_param(fet, HAL_PROTO_CONFIG_NO_BSL, 0); // is allowed to fail

 	printc_dbg("Check JTAG fuse...\n");
 	if (hal_proto_execute(&fet->hal, map_ver(fet, HAL_PROTO_FID_IS_JTAG_FUSE_BLOWN),
-			      NULL, 0) < 0)
+			      NULL, 0) < 0) {
 		return -1;
+	}
 	if ((fet->hal.length >= 2) &&
 	    (fet->hal.payload[0] == 0x55) &&
 	    (fet->hal.payload[1] == 0x55)) {
@ -1279,9 +1283,9 @@ int v3hil_configure(struct v3hil *fet)
 		      (fet->chip->features &
 		       CHIPINFO_FEATURE_1337) ? 1 : 0) < 0)
 		return -1;
-	/* is allowed to fail */
 	set_param(fet, HAL_PROTO_CONFIG_NO_BSL,
-		  (fet->chip->features & CHIPINFO_FEATURE_NO_BSL) ? 1 : 0);
+		  (fet->chip->features &
+		   CHIPINFO_FEATURE_NO_BSL) ? 1 : 0); // is allowed to fail

 	return 0;
 }
--- a/drivers/v3hil.h
+++ b/drivers/v3hil.h
@ -45,7 +45,7 @@ struct v3hil {
 	uint8_t			wdtctl;

 	/* Is this a v2 or v3 firmware running on the eZ-FET? */
-	uint16_t		proto_ver;
+	uint16_t			proto_ver;

 	/* Register cache: this must be flushed before restoring context
 	 * and updated after saving context.
--- a/ui/cmddb.c
+++ b/ui/cmddb.c
@ -190,12 +190,11 @@ const struct cmddb_record commands[] = {
 		.func = cmd_erase,
 		.help =
 "erase [all|segment] [address]\n"
-"erase segrange <address> <total-size> <seg-size>\n"
+"erase segrange <address> <size> <seg-size>\n"
 "    Erase the device under test. With no arguments, erases all of main\n"
 "    memory. Specify arguments to perform a mass erase, or to erase\n"
 "    individual segments. The \"segrange\" mode is used to erase an\n"
-"    address range via a series of segment erases, 'total-size' being"
-"    a multiple of 'seg-size'.\n"
+"    address range via a series of segment erases.\n"
 	},
 	{
 		.name = "step",
--- a/ui/devcmd.c
+++ b/ui/devcmd.c
@ -207,12 +207,6 @@ int cmd_erase(char **arg)
 					   "0x%x\n", segment_size);
 				return -1;
 			}
-
-			if (total_size % segment_size != 0) {
-				printc_err("erase: total size must be a multiple of the "
-				           "segment size!\n");
-				return -1;
-			}
 		} else {
 			printc_err("erase: unknown erase type: %s\n",
 				    type_text);
Author	SHA1	Message	Date
Triss	15edaf3c84	v3hil: fix for JTAGID=0x89 devices	2022-05-15 22:47:51 +02:00
Triss	b8ed9d7194	v3hil now working properly with firmware versions 2.3.0.6, 3.8.0.2 and 3.12.0.0 of eZ-FET on EXP430FR5994	2022-05-15 04:37:15 +02:00
Triss	fc79881203	v3hil: eZ-FET 3.8(r) working; 3.12(y) and 3.15 TODO	2022-05-12 14:32:26 +02:00
Triss	4db31c4287	version recognition	2022-05-09 02:45:48 +02:00
Triss	2ff6badecd	allow for '0X' hex prefix, too	2022-05-09 01:43:07 +02:00
Triss	3602ed39f9	fix MSP430FR5994 identification issue, handle NO_BSL config failure gracefully	2022-05-09 01:42:57 +02:00
Triss	dc77c43d80	ezfet3 fixes: now able to initialize etc., but the chip ID of my test MCU (FR5994) isn't being recognised	2022-05-09 01:32:43 +02:00