diff --git a/STM32_USB-FS-Device_Driver/inc/usb_core.h b/STM32_USB-FS-Device_Driver/inc/usb_core.h
index 6dfd0d1..0c62fcb 100644
--- a/STM32_USB-FS-Device_Driver/inc/usb_core.h
+++ b/STM32_USB-FS-Device_Driver/inc/usb_core.h
@@ -229,9 +229,9 @@ RESULT Standard_ClearFeature(void);
 void SetDeviceAddress(uint8_t);
 void NOP_Process(void);
 
-extern DEVICE_PROP Device_Property;
-extern  USER_STANDARD_REQUESTS User_Standard_Requests;
-extern  DEVICE  Device_Table;
+extern const DEVICE_PROP Device_Property;
+extern const USER_STANDARD_REQUESTS User_Standard_Requests;
+extern const DEVICE  Device_Table;
 extern DEVICE_INFO Device_Info;
 
 /* cells saving status during interrupt servicing */
diff --git a/src/Makefile b/src/Makefile
index a2ec1ad..93f8fd6 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -83,7 +83,7 @@ CSRC = $(PORTSRC) \
        $(CRYPTSRC) \
        main.c debug.c usb_lld.c \
 	hw_config.c usb_desc.c usb_prop.c \
-	usb-icc.c openpgp.c ac.c openpgp-do.c flash.c hardclock.c
+	usb-icc.c openpgp.c ac.c openpgp-do.c flash.c hardclock.c random.c
 
 # List ASM source files here
 ASMSRC = $(PORTASM) \
diff --git a/src/ac.c b/src/ac.c
index c1ba025..52c6f2f 100644
--- a/src/ac.c
+++ b/src/ac.c
@@ -2,6 +2,7 @@
  * ac.c -- Check access condition
  */
 
+#include "config.h"
 #include "ch.h"
 #include "gnuk.h"
 
diff --git a/src/flash.c b/src/flash.c
index 3545961..f94843a 100644
--- a/src/flash.c
+++ b/src/flash.c
@@ -25,6 +25,7 @@
  * Writing to Flash ROM is NOT YET IMPLEMENTED, just API only
  */
 
+#include "config.h"
 #include "ch.h"
 #include "gnuk.h"
 
diff --git a/src/gnuk.h b/src/gnuk.h
index 282aa79..7da2f5b 100644
--- a/src/gnuk.h
+++ b/src/gnuk.h
@@ -76,7 +76,6 @@ extern void flash_key_release (const uint8_t *);
 #define KEY_MAGIC_LEN 8
 #define KEY_CONTENT_LEN 256	/* p and q */
 #define GNUK_MAGIC "Gnuk KEY"
-#define KEYSTORE_LEN (KEY_MAGIC_LEN+4+4+KEY_CONTENT_LEN*2)
 
 /* encrypted data content */
 struct key_data {
@@ -96,8 +95,6 @@ struct prvkey_data {
   uint8_t dek_encrypted_3[DATA_ENCRYPTION_KEY_SIZE];
 };
 
-extern uint32_t get_random (void);
-
 extern int flash_key_write (uint8_t *key_addr, const uint8_t *key_data, const uint8_t *modulus);
 
 #define KEYSTRING_PASSLEN_SIZE  1
@@ -163,5 +160,7 @@ extern uint8_t keystring_md_pw3[KEYSTRING_MD_SIZE];
 /* 16-byte random bytes */
 extern uint8_t *get_data_encryption_key (void);
 extern void dek_free (uint8_t *);
+extern uint32_t get_random (void);
+extern void random_init (void);
 
 extern uint32_t hardclock (void);
diff --git a/src/gpg-data.c b/src/gpg-data.c
index d943999..23cc740 100644
--- a/src/gpg-data.c
+++ b/src/gpg-data.c
@@ -1,3 +1,4 @@
+#include "config.h"
 #include "ch.h"
 #include "gnuk.h"
 
diff --git a/src/gpg-data.s b/src/gpg-data.s
index f58a06c..6075c2c 100644
--- a/src/gpg-data.s
+++ b/src/gpg-data.s
@@ -193,7 +193,7 @@ get_data_rb_result:
 	.uleb128 0x7
 	.4byte	.LASF10
 	.byte	0x1
-	.byte	0x4
+	.byte	0x5
 	.4byte	0x97
 	.byte	0x1
 	.byte	0x5
@@ -211,7 +211,7 @@ get_data_rb_result:
 	.uleb128 0x7
 	.4byte	.LASF11
 	.byte	0x1
-	.byte	0x15
+	.byte	0x16
 	.4byte	0xbe
 	.byte	0x1
 	.byte	0x5
@@ -229,7 +229,7 @@ get_data_rb_result:
 	.uleb128 0x7
 	.4byte	.LASF12
 	.byte	0x1
-	.byte	0x1a
+	.byte	0x1b
 	.4byte	0xe5
 	.byte	0x1
 	.byte	0x5
@@ -247,7 +247,7 @@ get_data_rb_result:
 	.uleb128 0x7
 	.4byte	.LASF13
 	.byte	0x1
-	.byte	0x1f
+	.byte	0x20
 	.4byte	0x10c
 	.byte	0x1
 	.byte	0x5
@@ -265,7 +265,7 @@ get_data_rb_result:
 	.uleb128 0x7
 	.4byte	.LASF14
 	.byte	0x1
-	.byte	0x24
+	.byte	0x25
 	.4byte	0x133
 	.byte	0x1
 	.byte	0x5
@@ -283,7 +283,7 @@ get_data_rb_result:
 	.uleb128 0x7
 	.4byte	.LASF15
 	.byte	0x1
-	.byte	0x29
+	.byte	0x2a
 	.4byte	0x15a
 	.byte	0x1
 	.byte	0x5
@@ -301,7 +301,7 @@ get_data_rb_result:
 	.uleb128 0x7
 	.4byte	.LASF16
 	.byte	0x1
-	.byte	0x2f
+	.byte	0x30
 	.4byte	0x181
 	.byte	0x1
 	.byte	0x5
diff --git a/src/hardclock.c b/src/hardclock.c
index 6b60c3b..0ff0382 100644
--- a/src/hardclock.c
+++ b/src/hardclock.c
@@ -1,3 +1,4 @@
+#include "config.h"
 #include "ch.h"
 #include "hal.h"
 
diff --git a/src/main.c b/src/main.c
index 576a334..3cb8196 100644
--- a/src/main.c
+++ b/src/main.c
@@ -21,6 +21,8 @@
  *
  */
 
+#include "config.h"
+
 #include "ch.h"
 #include "hal.h"
 #include "usb_lld.h"
@@ -33,35 +35,16 @@
 #include "hw_config.h"
 #include "usb_pwr.h"
 
-Thread *blinker_thread;
-/*
- * Red LEDs blinker thread, times are in milliseconds.
- */
-static WORKING_AREA(waThread1, 128);
-static msg_t
-Thread1 (void *arg)
-{
-  (void)arg;
-  blinker_thread = chThdSelf ();
-  while (1)
-    {
-      palClearPad (IOPORT3, GPIOC_LED);
-      chEvtWaitOne (ALL_EVENTS);
-      palSetPad (IOPORT3, GPIOC_LED);
-      chEvtWaitOne (ALL_EVENTS);
-    }
-  return 0;
-}
-
 #ifdef DEBUG
-static
 struct stdout {
   Mutex m;
   CondVar start_cnd;
   CondVar finish_cnd;
   const char *str;
   int size;
-} stdout;
+};
+
+static struct stdout stdout;
 
 static void
 stdout_init (void)
@@ -93,10 +76,10 @@ extern uint32_t count_in;
 extern __IO uint32_t count_out;
 extern uint8_t buffer_in[VIRTUAL_COM_PORT_DATA_SIZE];
 extern uint8_t buffer_out[VIRTUAL_COM_PORT_DATA_SIZE];
-extern void USB_Init (void);
 
 static WORKING_AREA(waSTDOUTthread, 128);
-static msg_t STDOUTthread (void *arg)
+static msg_t
+STDOUTthread (void *arg)
 {
   (void)arg;
 
@@ -174,71 +157,17 @@ _write (const char *s, int size)
 }
 #endif
 
-static WORKING_AREA(waUSBthread, 128*2);
+static WORKING_AREA(waUSBthread, 128);
 extern msg_t USBthread (void *arg);
 
 static WORKING_AREA(waGPGthread, 128*16);
 extern msg_t GPGthread (void *arg);
 
+Thread *blinker_thread;
 /*
- * XXX: I have tried havege_rand, but it requires too much memory...
+ * Red LEDs blinker
  */
-/*
- * Multiply-with-carry method by George Marsaglia
- */
-static uint32_t m_w;
-static uint32_t m_z;
-
-static void
-random_init (void)
-{
-  static uint8_t s = 0;
-
- again:
-  switch ((s & 1))
-    {
-    case 0:
-      m_w = (m_w << 8) ^ hardclock ();
-      break;
-    case 1:
-      m_z = (m_z << 8) ^ hardclock ();
-      break;
-    }
-
-  s++;
-  if (m_w == 0 || m_z == 0)
-    goto again;
-}
-
-uint32_t
-get_random (void)
-{
-  m_z = 36969 * (m_z & 65535) + (m_z >> 16);
-  m_w = 18000 * (m_w & 65535) + (m_w >> 16);
-
-  return (m_z << 16) + m_w;
-}
-
-uint8_t dek[16];
-uint8_t *get_data_encryption_key (void)
-{
-  uint32_t r;
-  r = get_random ();
-  memcpy (dek, &r, 4);
-  r = get_random ();
-  memcpy (dek+4, &r, 4);
-  r = get_random ();
-  memcpy (dek+8, &r, 4);
-  r = get_random ();
-  memcpy (dek+12, &r, 4);
-  return dek;
-}
-
-void dek_free (uint8_t *dek)
-{
-  (void)dek;
-}
-
+#define EV_LED (eventmask_t)1
 
 /*
  * Entry point, note, the main() function is already a thread in the system
@@ -247,27 +176,26 @@ void dek_free (uint8_t *dek)
 int
 main (int argc, char **argv)
 {
+  eventmask_t m;
   int count = 0;
 
   (void)argc;
   (void)argv;
 
+  blinker_thread = chThdSelf ();
+
   gpg_do_table_init ();
 
   usb_lld_init ();
   USB_Init();
 
   stdout_init ();
-  /*
-   * Creates the blinker thread.
-   */
-  chThdCreateStatic (waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
 
 #ifdef DEBUG
   /*
    * Creates 'stdout' thread.
    */
-  chThdCreateStatic (waSTDOUTthread2, sizeof(waSTDOUTthread2), NORMALPRIO, STDOUTthread2, NULL);
+  chThdCreateStatic (waSTDOUTthread, sizeof(waSTDOUTthread), NORMALPRIO, STDOUTthread, NULL);
 #endif
 
   chThdCreateStatic (waUSBthread, sizeof(waUSBthread), NORMALPRIO, USBthread, NULL);
@@ -275,17 +203,23 @@ main (int argc, char **argv)
 
   while (1)
     {
+      uint8_t once = 0;
 #if 0
       if (palReadPad(IOPORT1, GPIOA_BUTTON))
 	palSetPad (IOPORT3, GPIOC_LED);
 #endif
 
-      chThdSleepMilliseconds (100);
+      m = chEvtWaitOneTimeout (ALL_EVENTS, 100);
+      if (m == EV_LED)
+	palClearPad (IOPORT3, GPIOC_LED);
 
-      if (bDeviceState != CONFIGURED)
-	random_init ();
+      if (once == 0 && bDeviceState == CONFIGURED)
+	{
+	  random_init ();
+	  once = 1;
+	}
 
-      if (bDeviceState == CONFIGURED && (count % 300) == 0)
+      if (bDeviceState == CONFIGURED && (count % 100) == 0)
 	{
 	  uint32_t r;
 	  r = get_random ();
@@ -295,6 +229,11 @@ main (int argc, char **argv)
 		  "Testing USB driver.\n\n"
 		  "Hello world\r\n\r\n", 47+21+15);
 	}
+
+      m = chEvtWaitOneTimeout (ALL_EVENTS, 100);
+      if (m == EV_LED)
+	palSetPad (IOPORT3, GPIOC_LED);
+
       count++;
     }
 
diff --git a/src/openpgp-do.c b/src/openpgp-do.c
index 105591a..28521b6 100644
--- a/src/openpgp-do.c
+++ b/src/openpgp-do.c
@@ -86,12 +86,14 @@ static const uint8_t const algorithm_attr[] __attribute__ ((aligned (1))) = {
   0x00		      /* 0: p&q , 3: CRT with N (not yet supported) */
 };
 
-const uint8_t const pw_status_bytes_template[] =
+static const uint8_t const do_pw_status_bytes_template[] =
 {
+  7,
   1,				/* PW1 valid for several PSO:CDS commands */
   127, 127, 127,		/* max length of PW1, RC, and PW3 */
   3, 0, 3			/* Error counter of PW1, RC, and PW3 */
 };
+#define PW_STATUS_BYTES_TEMPLATE (do_pw_status_bytes_template+1)
 
 #define SIZE_DIGITAL_SIGNATURE_COUNTER 3
 /* 3-byte binary (big endian) */
@@ -314,7 +316,7 @@ rw_pw_status (uint16_t tag, const uint8_t *data, int len, int is_write)
 	  flash_do_release (do_p->obj);
 	}
       else
-	memcpy (pwsb, pw_status_bytes_template, SIZE_PW_STATUS_BYTES);
+	memcpy (pwsb, PW_STATUS_BYTES_TEMPLATE, SIZE_PW_STATUS_BYTES);
 
       pwsb[0] = data[0];
       do_p->obj = flash_do_write (tag, pwsb, SIZE_PW_STATUS_BYTES);
@@ -329,24 +331,23 @@ rw_pw_status (uint16_t tag, const uint8_t *data, int len, int is_write)
     {
       const uint8_t *do_data;
 
-      if (with_tag)
-	{
-	  copy_tag (tag);
-	  *res_p++ = SIZE_PW_STATUS_BYTES;
-	}
-
       do_p = get_do_entry (GNUK_DO_PW_STATUS);
       do_data = (const uint8_t *)do_p->obj;
       if (do_data)
 	{
+	  if (with_tag)
+	    {
+	      copy_tag (tag);
+	      *res_p++ = SIZE_PW_STATUS_BYTES;
+	    }
+
 	  memcpy (res_p, &do_data[1], SIZE_PW_STATUS_BYTES);
 	  res_p += SIZE_PW_STATUS_BYTES;
+	  return 1;
 	}
       else
-	return -1;
+	return 0;
     }
-
-  return 0;
 }
 
 static aes_context aes;
@@ -397,7 +398,7 @@ proc_resetting_code (const uint8_t *data, int len)
     }
   else
     {
-      memcpy (pwsb, pw_status_bytes_template, SIZE_PW_STATUS_BYTES);
+      memcpy (pwsb, PW_STATUS_BYTES_TEMPLATE, SIZE_PW_STATUS_BYTES);
       pwsb[5] = 3;
     }
 
@@ -407,13 +408,13 @@ proc_resetting_code (const uint8_t *data, int len)
 static void
 encrypt (const uint8_t *key_str, uint8_t *data, int len)
 {
+  DEBUG_INFO ("ENC\r\n");
+  DEBUG_BINARY (data, len);
+
   aes_setkey_enc (&aes, key_str, 128);
   memset (iv, 0, 16);
   iv_offset = 0;
   aes_crypt_cfb128 (&aes, AES_ENCRYPT, len, &iv_offset, iv, data, data);
-
-  DEBUG_INFO ("ENC\r\n");
-  DEBUG_BINARY (data, KEYSTORE_LEN);
 }
 
 struct key_data kd;
@@ -425,8 +426,9 @@ decrypt (const uint8_t *key_str, uint8_t *data, int len)
   memset (iv, 0, 16);
   iv_offset = 0;
   aes_crypt_cfb128 (&aes, AES_DECRYPT, len, &iv_offset, iv, data, data);
+
   DEBUG_INFO ("DEC\r\n");
-  DEBUG_BINARY (data, KEYSTORE_LEN);
+  DEBUG_BINARY (data, len);
 }
 
 static uint16_t
@@ -466,7 +468,7 @@ gpg_do_load_prvkey (enum kind_of_key kk, int who, const uint8_t *keystring)
   memcpy (dek, ((uint8_t *)do_p->obj)+5+16*who, DATA_ENCRYPTION_KEY_SIZE);
 
   decrypt (keystring, dek, DATA_ENCRYPTION_KEY_SIZE);
-  decrypt (dek, (uint8_t *)&kd, sizeof (kd));
+  decrypt (dek, (uint8_t *)&kd, sizeof (struct key_data));
   if (memcmp (kd.magic, GNUK_MAGIC, KEY_MAGIC_LEN) != 0)
     return -1;
   /* XXX: more sanity check */
@@ -494,7 +496,6 @@ gpg_do_write_prvkey (enum kind_of_key kk, const uint8_t *key_data, int key_len,
   int r;
   struct do_table_entry *do_p;
   const uint8_t *modulus;
-  struct key_data *kd;
   struct prvkey_data *pd;
   uint8_t *key_addr;
   uint8_t *dek;
@@ -508,21 +509,13 @@ gpg_do_write_prvkey (enum kind_of_key kk, const uint8_t *key_data, int key_len,
 
   DEBUG_SHORT (key_len);
 
-  kd = (struct key_data *)malloc (sizeof (struct key_data));
-  if (kd == NULL)
-    return -1;
-
   pd = (struct prvkey_data *)malloc (sizeof (struct prvkey_data));
   if (pd == NULL)
-    {
-      free (kd);
-      return -1;
-    }
+    return -1;
 
   modulus = modulus_calc (key_data, key_len);
   if (modulus == NULL)
     {
-      free (kd);
       free (pd);
       return -1;
     }
@@ -530,37 +523,35 @@ gpg_do_write_prvkey (enum kind_of_key kk, const uint8_t *key_data, int key_len,
   key_addr = flash_key_alloc (kk);
   if (key_addr == NULL)
     {
-      free (kd);
       free (pd);
       modulus_free (modulus);
       return -1;
     }
 
-  memcpy (kd->data, key_data, KEY_CONTENT_LEN);
-  kd->check = calc_check32 (key_data, KEY_CONTENT_LEN);
-  kd->random = get_random ();
-  memcpy (kd->magic, GNUK_MAGIC, KEY_MAGIC_LEN);
+  memcpy (kd.data, key_data, KEY_CONTENT_LEN);
+  kd.check = calc_check32 (key_data, KEY_CONTENT_LEN);
+  kd.random = get_random ();
+  memcpy (kd.magic, GNUK_MAGIC, KEY_MAGIC_LEN);
 
   DEBUG_INFO ("enc...");
 
   dek = get_data_encryption_key (); /* 16-byte random bytes */
-  encrypt (dek, (uint8_t *)kd, sizeof (kd));
+  encrypt (dek, (uint8_t *)&kd, sizeof (struct key_data));
 
   DEBUG_INFO ("done\r\n");
 
-  r = flash_key_write (key_addr, kd->data, modulus);
+  r = flash_key_write (key_addr, kd.data, modulus);
   modulus_free (modulus);
 
   if (r < 0)
     {
       dek_free (dek);
       free (pd);
-      free (kd);
       return r;
     }
 
   pd->key_addr = key_addr;
-  memcpy (pd->crm_encrypted, (uint8_t *)&kd->check, ADDITIONAL_DATA_SIZE);
+  memcpy (pd->crm_encrypted, (uint8_t *)&kd.check, ADDITIONAL_DATA_SIZE);
 
   reset_pso_cds ();
   if (ks_pw1)
@@ -600,7 +591,6 @@ gpg_do_write_prvkey (enum kind_of_key kk, const uint8_t *key_data, int key_len,
   do_p->obj = p;
 
   dek_free (dek);
-  free (kd);
   free (pd);
   if (p == NULL)
     return -1;
@@ -726,20 +716,6 @@ gpg_do_table[] = {
   { GNUK_DO_KEYSTRING_PW3, DO_VAR, AC_NEVER, AC_NEVER, NULL },
   { GNUK_DO_KEYSTRING_RC, DO_VAR, AC_NEVER, AC_NEVER, NULL },
   { GNUK_DO_PW_STATUS, DO_VAR, AC_NEVER, AC_NEVER, NULL },
-  /* Pseudo DO READ: calculated */
-  { GPG_DO_HIST_BYTES, DO_PROC_READ, AC_ALWAYS, AC_NEVER, do_hist_bytes },
-  { GPG_DO_FP_ALL, DO_PROC_READ, AC_ALWAYS, AC_NEVER, do_fp_all },
-  { GPG_DO_CAFP_ALL, DO_PROC_READ, AC_ALWAYS, AC_NEVER, do_cafp_all },
-  { GPG_DO_KGTIME_ALL, DO_PROC_READ, AC_ALWAYS, AC_NEVER, do_kgtime_all },
-  /* Pseudo DO READ/WRITE: calculated */
-  { GPG_DO_PW_STATUS, DO_PROC_READWRITE, AC_ALWAYS, AC_ADMIN_AUTHORIZED,
-    rw_pw_status },
-  /* Fixed data */
-  { GPG_DO_AID, DO_FIXED, AC_ALWAYS, AC_NEVER, aid },
-  { GPG_DO_EXTCAP, DO_FIXED, AC_ALWAYS, AC_NEVER, extended_capabilities },
-  { GPG_DO_ALG_SIG, DO_FIXED, AC_ALWAYS, AC_NEVER, algorithm_attr },
-  { GPG_DO_ALG_DEC, DO_FIXED, AC_ALWAYS, AC_NEVER, algorithm_attr },
-  { GPG_DO_ALG_AUT, DO_FIXED, AC_ALWAYS, AC_NEVER, algorithm_attr },
   /* Variable(s): Fixed size, not changeable by user */
   { GPG_DO_DS_COUNT, DO_VAR, AC_ALWAYS, AC_NEVER, NULL },
   /* Variables: Fixed size */
@@ -759,6 +735,20 @@ gpg_do_table[] = {
   { GPG_DO_NAME, DO_VAR, AC_ALWAYS, AC_ADMIN_AUTHORIZED, NULL },
   { GPG_DO_LANGUAGE, DO_VAR, AC_ALWAYS, AC_ADMIN_AUTHORIZED, NULL },
   { GPG_DO_CH_CERTIFICATE, DO_VAR, AC_ALWAYS, AC_ADMIN_AUTHORIZED, NULL },
+  /* Pseudo DO READ: calculated */
+  { GPG_DO_HIST_BYTES, DO_PROC_READ, AC_ALWAYS, AC_NEVER, do_hist_bytes },
+  { GPG_DO_FP_ALL, DO_PROC_READ, AC_ALWAYS, AC_NEVER, do_fp_all },
+  { GPG_DO_CAFP_ALL, DO_PROC_READ, AC_ALWAYS, AC_NEVER, do_cafp_all },
+  { GPG_DO_KGTIME_ALL, DO_PROC_READ, AC_ALWAYS, AC_NEVER, do_kgtime_all },
+  /* Pseudo DO READ/WRITE: calculated */
+  { GPG_DO_PW_STATUS, DO_PROC_READWRITE, AC_ALWAYS, AC_ADMIN_AUTHORIZED,
+    rw_pw_status },
+  /* Fixed data */
+  { GPG_DO_AID, DO_FIXED, AC_ALWAYS, AC_NEVER, aid },
+  { GPG_DO_EXTCAP, DO_FIXED, AC_ALWAYS, AC_NEVER, extended_capabilities },
+  { GPG_DO_ALG_SIG, DO_FIXED, AC_ALWAYS, AC_NEVER, algorithm_attr },
+  { GPG_DO_ALG_DEC, DO_FIXED, AC_ALWAYS, AC_NEVER, algorithm_attr },
+  { GPG_DO_ALG_AUT, DO_FIXED, AC_ALWAYS, AC_NEVER, algorithm_attr },
   /* Compound data: Read access only */
   { GPG_DO_CH_DATA, DO_CN_READ, AC_ALWAYS, AC_NEVER, cn_ch_data },
   { GPG_DO_APP_DATA, DO_CN_READ, AC_ALWAYS, AC_NEVER, cn_app_data },
@@ -790,7 +780,7 @@ gpg_do_table_init (void)
   do_p = get_do_entry (GPG_DO_LOGIN_DATA);
   do_p->obj = do_5e;
   do_p = get_do_entry (GNUK_DO_PW_STATUS);
-  do_p->obj = pw_status_bytes_template;
+  do_p->obj = do_pw_status_bytes_template;
   do_p = get_do_entry (GPG_DO_NAME);
   do_p->obj = do_5b;
   do_p = get_do_entry (GPG_DO_LANGUAGE);
@@ -1077,7 +1067,12 @@ gpg_do_read_simple (uint16_t tag)
   if (do_data == NULL)
     return NULL;
 
-  return do_data+1;
+  if (do_data[0] < 128)
+    return do_data+1;
+  else if (do_data[0] == 0x81)
+    return do_data+2;
+  else				/* 0x82 */
+    return do_data+3;
 }
 
 void
diff --git a/src/openpgp.c b/src/openpgp.c
index 8700295..59ec263 100644
--- a/src/openpgp.c
+++ b/src/openpgp.c
@@ -21,6 +21,7 @@
  *
  */
 
+#include "config.h"
 #include "ch.h"
 #include "hal.h"
 #include "gnuk.h"
@@ -475,7 +476,7 @@ struct command
   void (*cmd_handler) (void);
 };
 
-struct command cmds[] = {
+const struct command cmds[] = {
   { INS_VERIFY, cmd_verify },
   { INS_CHANGE_REFERENCE_DATA, cmd_change_password },
   { INS_PSO, cmd_pso },
diff --git a/src/rsa-sign.c b/src/rsa-sign.c
index 1ee2e22..5c07733 100644
--- a/src/rsa-sign.c
+++ b/src/rsa-sign.c
@@ -1,9 +1,10 @@
+#include "config.h"
 #include "ch.h"
 #include "gnuk.h"
 #include "polarssl/config.h"
 #include "polarssl/rsa.h"
 
-static rsa_context ctx;
+static rsa_context rsa_ctx;
 
 int
 rsa_sign (const uint8_t *raw_message, uint8_t *output, int msg_len)
@@ -11,38 +12,40 @@ rsa_sign (const uint8_t *raw_message, uint8_t *output, int msg_len)
   mpi P1, Q1, H;
   int r;
 
-  mpi_init( &P1, &Q1, &H, NULL );
-  rsa_init( &ctx, RSA_PKCS_V15, 0 );
+  DEBUG_BINARY (kd.data, 256);
 
-  ctx.len = 2048 / 8;
-  mpi_read_string ( &ctx.E, 16, "10001" );
-  mpi_read_binary ( &ctx.P, &kd.data[0], ctx.len / 2 );
-  mpi_read_binary ( &ctx.Q, &kd.data[128], ctx.len / 2 );
-  mpi_mul_mpi ( &ctx.N, &ctx.P, &ctx.Q );
-  mpi_sub_int ( &P1, &ctx.P, 1 );
-  mpi_sub_int ( &Q1, &ctx.Q, 1 );
-  mpi_mul_mpi ( &H, &P1, &Q1 );
-  mpi_inv_mod ( &ctx.D , &ctx.E, &H  );
-  mpi_mod_mpi ( &ctx.DP, &ctx.D, &P1);
-  mpi_mod_mpi ( &ctx.DQ, &ctx.D, &Q1);
-  mpi_inv_mod ( &ctx.QP, &ctx.Q, &ctx.P);
+  mpi_init (&P1, &Q1, &H, NULL);
+  rsa_init (&rsa_ctx, RSA_PKCS_V15, 0);
+
+  rsa_ctx.len = 2048 / 8;
+  mpi_read_string (&rsa_ctx.E, 16, "10001");
+  mpi_read_binary (&rsa_ctx.P, &kd.data[0], rsa_ctx.len / 2);
+  mpi_read_binary (&rsa_ctx.Q, &kd.data[128], rsa_ctx.len / 2);
+  mpi_mul_mpi (&rsa_ctx.N, &rsa_ctx.P, &rsa_ctx.Q);
+  mpi_sub_int (&P1, &rsa_ctx.P, 1);
+  mpi_sub_int (&Q1, &rsa_ctx.Q, 1);
+  mpi_mul_mpi (&H, &P1, &Q1);
+  mpi_inv_mod (&rsa_ctx.D , &rsa_ctx.E, &H);
+  mpi_mod_mpi (&rsa_ctx.DP, &rsa_ctx.D, &P1);
+  mpi_mod_mpi (&rsa_ctx.DQ, &rsa_ctx.D, &Q1);
+  mpi_inv_mod (&rsa_ctx.QP, &rsa_ctx.Q, &rsa_ctx.P);
   mpi_free (&P1, &Q1, &H, NULL);
 
   DEBUG_INFO ("RSA...");
 
-  if ((r = rsa_check_privkey ( &ctx )) == 0)
+  if ((r = rsa_check_privkey (&rsa_ctx)) == 0)
     DEBUG_INFO ("ok...");
   else
     {
       DEBUG_INFO ("failed.\r\n");
-      DEBUG_BYTE (r);
-      rsa_free (&ctx);
+      DEBUG_SHORT (r);
+      rsa_free (&rsa_ctx);
       return r;
     }
 
-  r = rsa_pkcs1_sign ( &ctx, RSA_PRIVATE, SIG_RSA_RAW,
-		       msg_len, raw_message, output );
-  rsa_free (&ctx);
+  r = rsa_pkcs1_sign (&rsa_ctx, RSA_PRIVATE, SIG_RSA_RAW,
+		      msg_len, raw_message, output);
+  rsa_free (&rsa_ctx);
   DEBUG_INFO ("done.\r\n");
   if (r < 0)
     {
diff --git a/src/usb-icc.c b/src/usb-icc.c
index e89794c..86d6bf4 100644
--- a/src/usb-icc.c
+++ b/src/usb-icc.c
@@ -21,11 +21,10 @@
  *
  */
 
+#include "config.h"
 #include "ch.h"
 #include "hal.h"
-
 #include "gnuk.h"
-
 #include "usb_lib.h"
 #include "usb_desc.h"
 #include "usb_mem.h"
diff --git a/src/usb_desc.c b/src/usb_desc.c
index b8516ac..ffe7d96 100644
--- a/src/usb_desc.c
+++ b/src/usb_desc.c
@@ -193,12 +193,12 @@ ONE_DESCRIPTOR Device_Descriptor = {
   sizeof (gnukDeviceDescriptor)
 };
 
-ONE_DESCRIPTOR Config_Descriptor = {
+const ONE_DESCRIPTOR Config_Descriptor = {
   (uint8_t*)gnukConfigDescriptor,
   sizeof (gnukConfigDescriptor)
 };
 
-ONE_DESCRIPTOR String_Descriptor[4] = {
+const ONE_DESCRIPTOR String_Descriptor[4] = {
   {(uint8_t*)gnukStringLangID, sizeof (gnukStringLangID)},
   {(uint8_t*)gnukStringVendor, sizeof (gnukStringVendor)},
   {(uint8_t*)gnukStringProduct, sizeof (gnukStringProduct)},
diff --git a/src/usb_prop.c b/src/usb_prop.c
index e8df21f..1a822e0 100644
--- a/src/usb_prop.c
+++ b/src/usb_prop.c
@@ -182,7 +182,7 @@ gnuk_device_Get_Interface_Setting (uint8_t Interface, uint8_t AlternateSetting)
  * Interface to USB core
  */
 
-DEVICE_PROP Device_Property = {
+const DEVICE_PROP Device_Property = {
   gnuk_device_init,
   gnuk_device_reset,
   gnuk_device_Status_In,
@@ -202,12 +202,12 @@ DEVICE_PROP Device_Property = {
   GNUK_MAX_PACKET_SIZE
 };
 
-DEVICE Device_Table = {
+const DEVICE Device_Table = {
   EP_NUM,
   1
 };
 
-USER_STANDARD_REQUESTS User_Standard_Requests = {
+const USER_STANDARD_REQUESTS User_Standard_Requests = {
   NOP_Process,			/* GetConfiguration */ 
   gnuk_device_SetConfiguration,
   NOP_Process,			/* GetInterface */