diff --git a/arch/arm/src/stm32/chip/stm32_flash.h b/arch/arm/src/stm32/chip/stm32_flash.h
index 6bc1085bd18..13c966c05c2 100644
--- a/arch/arm/src/stm32/chip/stm32_flash.h
+++ b/arch/arm/src/stm32/chip/stm32_flash.h
@@ -59,16 +59,33 @@
 
 #if defined(CONFIG_STM32_FLASH_CONFIG_DEFAULT)
 #  if defined(CONFIG_STM32_STM32L15XX)
+#    if defined(CONFIG_STM32_HIGHDENSITY)
 
-/* The STM32 L15xx/L16xx can support up to 384KB of FLASH.  (In reality, supported
- * L15xx parts have no more than 128KB). The program memory block is divided into
- * 96 sectors of 4 Kbytes each, and each sector is further split up into 16 pages of
- * 256 bytes each. The sector is the write protection granularity. In total, the
+/* Different STM32L1xxx MCU version are now called by different 'categories' instead
+ * of 'densities'. Cat.5 MCU can have up to 512KB of FLASH. STM32L1xxx also have
+ * data EEPROM, up to 16KB.
+ */
+
+#      define STM32_FLASH_NPAGES        2048
+#      define STM32_FLASH_PAGESIZE      256
+#    else
+
+/* The STM32 (< Cat.5) L15xx/L16xx can support up to 384KB of FLASH. (In reality, most
+ * supported L15xx parts have no more than 128KB). The program memory block is divided
+ * into 96 sectors of 4 Kbytes each, and each sector is further split up into 16 pages
+ * of 256 bytes each. The sector is the write protection granularity. In total, the
  * program memory block contains 1536 pages.
  */
 
-#    define STM32_FLASH_NPAGES        1536
-#    define STM32_FLASH_PAGESIZE      256
+#      define STM32_FLASH_NPAGES        1536
+#      define STM32_FLASH_PAGESIZE      256
+#    endif
+
+     /* Maximum EEPROM size on Cat.5 MCU. TODO: this should be in chip config. */
+
+#    ifndef STM32_EEPROM_SIZE
+#      define STM32_EEPROM_SIZE         (16 * 1024)
+#    endif
 
 #  elif defined(CONFIG_STM32_LOWDENSITY)
 #    define STM32_FLASH_NPAGES        32
@@ -201,27 +218,40 @@
 #    elif defined(CONFIG_STM32_FLASH_CONFIG_I)
 #    endif
 #  endif
-#endif
+#endif /* !defined(CONFIG_STM32_FLASH_CONFIG_DEFAULT) */
 
 #ifdef STM32_FLASH_PAGESIZE
 #  define STM32_FLASH_SIZE            (STM32_FLASH_NPAGES * STM32_FLASH_PAGESIZE)
-#endif /* def STM32_FLASH_PAGESIZE */
+#endif
 
 /* Register Offsets *****************************************************************/
 
-#define STM32_FLASH_ACR_OFFSET     0x0000
-#define STM32_FLASH_KEYR_OFFSET    0x0004
-#define STM32_FLASH_OPTKEYR_OFFSET 0x0008
-#define STM32_FLASH_SR_OFFSET      0x000c
-#define STM32_FLASH_CR_OFFSET      0x0010
-
-#if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX) || \
-    defined(CONFIG_STM32_STM32F33XX) || defined(CONFIG_STM32_STM32F37XX)
-#  define STM32_FLASH_AR_OFFSET    0x0014
-#  define STM32_FLASH_OBR_OFFSET   0x001c
-#  define STM32_FLASH_WRPR_OFFSET  0x0020
-#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
-#  define STM32_FLASH_OPTCR_OFFSET 0x0014
+#define STM32_FLASH_ACR_OFFSET       0x0000
+#if defined(CONFIG_STM32_STM32L15XX)
+#  define STM32_FLASH_PECR_OFFSET    0x0004
+#  define STM32_FLASH_PDKEYR_OFFSET  0x0008
+#  define STM32_FLASH_PEKEYR_OFFSET  0x000c
+#  define STM32_FLASH_PRGKEYR_OFFSET 0x0010
+#  define STM32_FLASH_OPTKEYR_OFFSET 0x0014
+#  define STM32_FLASH_SR_OFFSET      0x0018
+#  define STM32_FLASH_OBR_OFFSET     0x001c
+#  define STM32_FLASH_WRPR1_OFFSET   0x0020
+#  define STM32_FLASH_WRPR2_OFFSET   0x0080
+#  define STM32_FLASH_WRPR3_OFFSET   0x0084
+#  define STM32_FLASH_WRPR4_OFFSET   0x0088
+#else
+#  define STM32_FLASH_KEYR_OFFSET    0x0004
+#  define STM32_FLASH_OPTKEYR_OFFSET 0x0008
+#  define STM32_FLASH_SR_OFFSET      0x000c
+#  define STM32_FLASH_CR_OFFSET      0x0010
+#  if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX) || \
+      defined(CONFIG_STM32_STM32F33XX) || defined(CONFIG_STM32_STM32F37XX)
+#    define STM32_FLASH_AR_OFFSET    0x0014
+#    define STM32_FLASH_OBR_OFFSET   0x001c
+#    define STM32_FLASH_WRPR_OFFSET  0x0020
+#  elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
+#    define STM32_FLASH_OPTCR_OFFSET 0x0014
+#  endif
 #endif
 
 #if defined(CONFIG_STM32_STM32F427) || defined(CONFIG_STM32_STM32F429)
@@ -230,22 +260,36 @@
 
 /* Register Addresses ***************************************************************/
 
-#define STM32_FLASH_ACR            (STM32_FLASHIF_BASE+STM32_FLASH_ACR_OFFSET)
-#define STM32_FLASH_KEYR           (STM32_FLASHIF_BASE+STM32_FLASH_KEYR_OFFSET)
-#define STM32_FLASH_OPTKEYR        (STM32_FLASHIF_BASE+STM32_FLASH_OPTKEYR_OFFSET)
-#define STM32_FLASH_SR             (STM32_FLASHIF_BASE+STM32_FLASH_SR_OFFSET)
-#define STM32_FLASH_CR             (STM32_FLASHIF_BASE+STM32_FLASH_CR_OFFSET)
+#define STM32_FLASH_ACR              (STM32_FLASHIF_BASE+STM32_FLASH_ACR_OFFSET)
+#if defined(CONFIG_STM32_STM32L15XX)
+#  define STM32_FLASH_PECR           (STM32_FLASHIF_BASE+STM32_FLASH_PECR_OFFSET)
+#  define STM32_FLASH_PDKEYR         (STM32_FLASHIF_BASE+STM32_FLASH_PDKEYR_OFFSET)
+#  define STM32_FLASH_PEKEYR         (STM32_FLASHIF_BASE+STM32_FLASH_PEKEYR_OFFSET)
+#  define STM32_FLASH_PRGKEYR        (STM32_FLASHIF_BASE+STM32_FLASH_PRGKEYR_OFFSET)
+#  define STM32_FLASH_OPTKEYR        (STM32_FLASHIF_BASE+STM32_FLASH_OPTKEYR_OFFSET)
+#  define STM32_FLASH_SR             (STM32_FLASHIF_BASE+STM32_FLASH_SR_OFFSET)
+#  define STM32_FLASH_OBR            (STM32_FLASHIF_BASE+STM32_FLASH_OBR_OFFSET)
+#  define STM32_FLASH_WRPR1          (STM32_FLASHIF_BASE+STM32_FLASH_WRPR1_OFFSET)
+#  define STM32_FLASH_WRPR2          (STM32_FLASHIF_BASE+STM32_FLASH_WRPR2_OFFSET)
+#  define STM32_FLASH_WRPR3          (STM32_FLASHIF_BASE+STM32_FLASH_WRPR3_OFFSET)
+#  define STM32_FLASH_WRPR4          (STM32_FLASHIF_BASE+STM32_FLASH_WRPR4_OFFSET)
+#else
+#  define STM32_FLASH_KEYR           (STM32_FLASHIF_BASE+STM32_FLASH_KEYR_OFFSET)
+#  define STM32_FLASH_OPTKEYR        (STM32_FLASHIF_BASE+STM32_FLASH_OPTKEYR_OFFSET)
+#  define STM32_FLASH_SR             (STM32_FLASHIF_BASE+STM32_FLASH_SR_OFFSET)
+#  define STM32_FLASH_CR             (STM32_FLASHIF_BASE+STM32_FLASH_CR_OFFSET)
 
-#if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX) || \
-    defined(CONFIG_STM32_STM32F33XX) || defined(CONFIG_STM32_STM32F37XX)
-#  define STM32_FLASH_AR           (STM32_FLASHIF_BASE+STM32_FLASH_AR_OFFSET)
-#  define STM32_FLASH_OBR          (STM32_FLASHIF_BASE+STM32_FLASH_OBR_OFFSET)
-#  define STM32_FLASH_WRPR         (STM32_FLASHIF_BASE+STM32_FLASH_WRPR_OFFSET)
-#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
-#  define STM32_FLASH_OPTCR        (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR_OFFSET)
-#endif
-#if defined(CONFIG_STM32_STM32F427) || defined(CONFIG_STM32_STM32F429)
-#  define STM32_FLASH_OPTCR1       (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR1_OFFSET)
+#  if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX) || \
+      defined(CONFIG_STM32_STM32F33XX) || defined(CONFIG_STM32_STM32F37XX)
+#    define STM32_FLASH_AR           (STM32_FLASHIF_BASE+STM32_FLASH_AR_OFFSET)
+#    define STM32_FLASH_OBR          (STM32_FLASHIF_BASE+STM32_FLASH_OBR_OFFSET)
+#    define STM32_FLASH_WRPR         (STM32_FLASHIF_BASE+STM32_FLASH_WRPR_OFFSET)
+#  elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX)
+#    define STM32_FLASH_OPTCR        (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR_OFFSET)
+#  endif
+#  if defined(CONFIG_STM32_STM32F427) || defined(CONFIG_STM32_STM32F429)
+#    define STM32_FLASH_OPTCR1       (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR1_OFFSET)
+#  endif
 #endif
 
 /* Register Bitfield Definitions ****************************************************/
@@ -303,6 +347,34 @@
 #  define FLASH_SR_PGPERR           (1 << 6)  /* Bit 6: Programming parallelism error */
 #  define FLASH_SR_PGSERR           (1 << 7)  /* Bit 7: Programming sequence error */
 #  define FLASH_SR_BSY              (1 << 16) /* Bit 16: Busy */
+#elif defined(CONFIG_STM32_STM32L15XX)
+#  define FLASH_SR_BSY              (1 << 0)  /* Bit 0: Busy */
+#  define FLASH_SR_EOP              (1 << 1)  /* Bit 1: End of operation */
+#  define FLASH_SR_ENDHV            (1 << 2)  /* Bit 2: End of high voltage */
+#  define FLASH_SR_READY            (1 << 3)  /* Bit 3: Flash memory module ready after low power mode */
+#  define FLASH_SR_WRPERR           (1 << 8)  /* Bit 8: Write protection error */
+#  define FLASH_SR_PGAERR           (1 << 9)  /* Bit 9: Programming alignment error  */
+#  define FLASH_SR_SIZERR           (1 << 10) /* Bit 10: Size error */
+#  define FLASH_SR_OPTVERR          (1 << 11) /* Bit 11: Option validity error */
+#  define FLASH_SR_OPTVERRUSR       (1 << 12) /* Bit 12: Option UserValidity Error */
+#  define FLASH_SR_RDERR            (1 << 13) /* Bit 13: Read protected error */
+#endif
+
+/* Program/Erase Control Register (PECR) */
+
+#if defined(CONFIG_STM32_STM32L15XX)
+#  define FLASH_PECR_PELOCK         (1 << 0)  /* Bit 0: PECR and data EEPROM lock */
+#  define FLASH_PECR_PRGLOCK        (1 << 1)  /* Bit 1: Program memory lock */
+#  define FLASH_PECR_OPTLOCK        (1 << 2)  /* Bit 2: Option bytes block lock */
+#  define FLASH_PECR_PROG           (1 << 3)  /* Bit 3: Program memory selection */
+#  define FLASH_PECR_DATA           (1 << 4)  /* Bit 4: Data EEPROM selection */
+#  define FLASH_PECR_FTDW           (1 << 8)  /* Bit 8: Fixed time data write for Byte, Half Word and Word programming */
+#  define FLASH_PECR_ERASE          (1 << 9)  /* Bit 9: Page or Double Word erase mode */
+#  define FLASH_PECR_FPRG           (1 << 10) /* Bit 10: Half Page/Double Word programming mode */
+#  define FLASH_PECR_PARALLBANK     (1 << 15) /* Bit 15: Parallel bank mode */
+#  define FLASH_PECR_EOPIE          (1 << 16) /* Bit 16: End of programming interrupt enable */
+#  define FLASH_PECR_ERRIE          (1 << 17) /* Bit 17: Error interrupt enable */
+#  define FLASH_PECR_OBL_LAUNCH     (1 << 18) /* Bit 18: Launch the option byte loading */
 #endif
 
 /* Flash Control Register (CR) */
@@ -380,7 +452,6 @@
 
 #  define FLASH_OPTCR1_BFB2_SHIFT    (4)                   /* Bits 4: Dual-bank Boot option byte */
 #  define FLASH_OPTCR1_BFB2_MASK     (1 << FLASH_OPTCR_NWRP_SHIFT)
-
 #endif
 
 #if defined(CONFIG_STM32_STM32F446)
diff --git a/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h b/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h
index 82fb74a4481..d44c664edbf 100644
--- a/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h
+++ b/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h
@@ -424,6 +424,10 @@
 #define GPIO_I2C3_SCL_2       (GPIO_ALT|GPIO_AF4|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTH|GPIO_PIN7)
 #define GPIO_I2C3_SDA_1       (GPIO_ALT|GPIO_AF4|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN9)
 #define GPIO_I2C3_SDA_2       (GPIO_ALT|GPIO_AF4|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTH|GPIO_PIN8)
+#if defined(CONFIG_STM32_STM32F411)
+#  define GPIO_I2C3_SDA_3     (GPIO_ALT|GPIO_AF9|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN4)
+#  define GPIO_I2C3_SDA_4     (GPIO_ALT|GPIO_AF9|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN8)  
+#endif
 #define GPIO_I2C3_SMBA_1      (GPIO_ALT|GPIO_AF4|GPIO_SPEED_50MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN9)
 #define GPIO_I2C3_SMBA_2      (GPIO_ALT|GPIO_AF4|GPIO_SPEED_50MHz|GPIO_PUSHPULL|GPIO_PORTH|GPIO_PIN9)
 #if defined(CONFIG_STM32_STM32F446)
diff --git a/arch/arm/src/stm32/stm32_flash.c b/arch/arm/src/stm32/stm32_flash.c
index fb1e1cac220..1dae18724c5 100644
--- a/arch/arm/src/stm32/stm32_flash.c
+++ b/arch/arm/src/stm32/stm32_flash.c
@@ -59,10 +59,10 @@
 
 #include "up_arch.h"
 
-/* Only for the STM32F[1|3|4]0xx family for now */
+/* Only for the STM32F[1|3|4]0xx family and STM32L15xx (EEPROM only) for now */
 
 #if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX) || \
-    defined (CONFIG_STM32_STM32F40XX)
+    defined (CONFIG_STM32_STM32F40XX) || defined(CONFIG_STM32_STM32L15XX)
 
 #if defined(CONFIG_STM32_FLASH_CONFIG_DEFAULT) && \
     (defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX))
@@ -73,15 +73,25 @@
  * Pre-processor Definitions
  ************************************************************************************/
 
-#define FLASH_KEY1      0x45670123
-#define FLASH_KEY2      0xCDEF89AB
+#if defined(CONFIG_STM32_STM32L15XX)
+#  define FLASH_KEY1      0x8C9DAEBF
+#  define FLASH_KEY2      0x13141516
+#else
+#  define FLASH_KEY1      0x45670123
+#  define FLASH_KEY2      0xCDEF89AB
+#endif
 
 #if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX)
-#define FLASH_CR_PAGE_ERASE              FLASH_CR_PER
-#define FLASH_SR_WRITE_PROTECTION_ERROR  FLASH_SR_WRPRT_ERR
+#  define FLASH_CR_PAGE_ERASE              FLASH_CR_PER
+#  define FLASH_SR_WRITE_PROTECTION_ERROR  FLASH_SR_WRPRT_ERR
 #elif defined(CONFIG_STM32_STM32F40XX)
-#define FLASH_CR_PAGE_ERASE              FLASH_CR_SER
-#define FLASH_SR_WRITE_PROTECTION_ERROR  FLASH_SR_WRPERR
+#  define FLASH_CR_PAGE_ERASE              FLASH_CR_SER
+#  define FLASH_SR_WRITE_PROTECTION_ERROR  FLASH_SR_WRPERR
+#endif
+
+#if defined(CONFIG_STM32_STM32L15XX)
+#  define EEPROM_KEY1     0x89ABCDEF
+#  define EEPROM_KEY2     0x02030405
 #endif
 
 /************************************************************************************
@@ -107,6 +117,8 @@ static inline void sem_unlock(void)
   sem_post(&g_sem);
 }
 
+#if !defined(CONFIG_STM32_STM32L15XX)
+
 static void flash_unlock(void)
 {
   while (getreg32(STM32_FLASH_SR) & FLASH_SR_BSY)
@@ -128,6 +140,8 @@ static void flash_lock(void)
   modifyreg32(STM32_FLASH_CR, 0, FLASH_CR_LOCK);
 }
 
+#endif /* !defined(CONFIG_STM32_STM32L15XX) */
+
 #if defined(CONFIG_STM32_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW)
 static void data_cache_disable(void)
 {
@@ -146,6 +160,183 @@ static void data_cache_enable(void)
 }
 #endif /* defined(CONFIG_STM32_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW) */
 
+#if defined(CONFIG_STM32_STM32L15XX)
+
+static void stm32_eeprom_unlock(void)
+{
+  while (getreg32(STM32_FLASH_SR) & FLASH_SR_BSY)
+    {
+      up_waste();
+    }
+
+  if (getreg32(STM32_FLASH_PECR) & FLASH_PECR_PELOCK)
+    {
+      /* Unlock sequence */
+
+      putreg32(EEPROM_KEY1, STM32_FLASH_PEKEYR);
+      putreg32(EEPROM_KEY2, STM32_FLASH_PEKEYR);
+    }
+}
+
+static void stm32_eeprom_lock(void)
+{
+  modifyreg32(STM32_FLASH_PECR, 0, FLASH_PECR_PELOCK);
+}
+
+static void flash_unlock(void)
+{
+  if (getreg32(STM32_FLASH_PECR) & FLASH_PECR_PRGLOCK)
+    {
+      stm32_eeprom_unlock();
+
+      /* Unlock sequence */
+
+      putreg32(FLASH_KEY1, STM32_FLASH_PRGKEYR);
+      putreg32(FLASH_KEY2, STM32_FLASH_PRGKEYR);
+    }
+}
+
+static void flash_lock(void)
+{
+  modifyreg32(STM32_FLASH_PECR, 0, FLASH_PECR_PRGLOCK);
+  stm32_eeprom_lock();
+}
+
+static ssize_t stm32_eeprom_erase_write(size_t addr, const void *buf,
+                                        size_t buflen)
+{
+  const char *cbuf = buf;
+  size_t i;
+
+  if (buflen == 0)
+    {
+      return 0;
+    }
+
+  /* Check for valid address range */
+
+  if (addr >= STM32_EEPROM_BASE)
+    {
+      addr -= STM32_EEPROM_BASE;
+    }
+
+  if (addr >= STM32_EEPROM_SIZE)
+    {
+      return -EINVAL;
+    }
+
+  /* TODO: Voltage range must be range 1 or 2. Erase/program not allowed in
+   * range 3.
+   */
+
+  stm32_eeprom_unlock();
+
+  /* Clear pending status flags. */
+
+  putreg32(FLASH_SR_WRPERR | FLASH_SR_PGAERR |
+           FLASH_SR_SIZERR | FLASH_SR_OPTVERR |
+           FLASH_SR_OPTVERRUSR | FLASH_SR_RDERR, STM32_FLASH_SR);
+
+  /* Enable automatic erasing (by disabling 'fixed time' programming). */
+
+  modifyreg32(STM32_FLASH_PECR, FLASH_PECR_FTDW, 0);
+
+  /* Write buffer to EEPROM data memory. */
+
+  addr += STM32_EEPROM_BASE;
+  i = 0;
+  while (i < buflen)
+    {
+      uint32_t writeval;
+      size_t left = buflen - i;
+
+      if ((addr & 0x03) == 0x00 && left >= 4)
+        {
+          /* Read/erase/write word */
+
+          writeval = cbuf ? *(uint32_t *)cbuf : 0;
+          putreg32(writeval, addr);
+        }
+      else if ((addr & 0x01) == 0x00 && left >= 2)
+        {
+          /* Read/erase/write half-word */
+
+          writeval = cbuf ? *(uint16_t *)cbuf : 0;
+          putreg16(writeval, addr);
+        }
+      else
+        {
+          /* Read/erase/write byte */
+
+          writeval = cbuf ? *(uint8_t *)cbuf : 0;
+          putreg8(writeval, addr);
+        }
+
+      /* ... and wait to complete. */
+
+      while (getreg32(STM32_FLASH_SR) & FLASH_SR_BSY)
+        {
+          up_waste();
+        }
+
+      /* Verify */
+
+      /* We do not check Options Byte invalid flags FLASH_SR_OPTVERR
+       * and FLASH_SR_OPTVERRUSR for EEPROM erase/write. They are unrelated
+       * and STM32L standard library does not check for these either.
+       */
+
+      if (getreg32(STM32_FLASH_SR) & (FLASH_SR_WRPERR | FLASH_SR_PGAERR |
+                                      FLASH_SR_SIZERR | FLASH_SR_RDERR))
+        {
+          stm32_eeprom_lock();
+          return -EROFS;
+        }
+
+      if ((addr & 0x03) == 0x00 && left >= 4)
+        {
+          if (getreg32(addr) != writeval)
+            {
+              stm32_eeprom_lock();
+              return -EIO;
+            }
+
+          addr += 4;
+          i += 4;
+          cbuf += !!(cbuf) * 4;
+        }
+      else if ((addr & 0x01) == 0x00 && left >= 2)
+        {
+          if (getreg16(addr) != writeval)
+            {
+              stm32_eeprom_lock();
+              return -EIO;
+            }
+
+          addr += 2;
+          i += 2;
+          cbuf += !!(cbuf) * 2;
+        }
+      else
+        {
+          if (getreg8(addr) != writeval)
+            {
+              stm32_eeprom_lock();
+              return -EIO;
+            }
+
+          addr += 1;
+          i += 1;
+          cbuf += !!(cbuf) * 1;
+        }
+    }
+
+  stm32_eeprom_lock();
+  return buflen;
+}
+
+#endif /* defined(CONFIG_STM32_STM32L15XX) */
+
 /************************************************************************************
  * Public Functions
  ************************************************************************************/
@@ -164,6 +355,35 @@ void stm32_flash_lock(void)
   sem_unlock();
 }
 
+#if defined(CONFIG_STM32_STM32L15XX)
+
+size_t stm32_eeprom_size(void)
+{
+  return STM32_EEPROM_SIZE;
+}
+
+size_t stm32_eeprom_getaddress(void)
+{
+  return STM32_EEPROM_BASE;
+}
+
+ssize_t stm32_eeprom_write(size_t addr, const void *buf, size_t buflen)
+{
+  if (!buf)
+    {
+      return -EINVAL;
+    }
+
+  return stm32_eeprom_erase_write(addr, buf, buflen);
+}
+
+ssize_t stm32_eeprom_erase(size_t addr, size_t eraselen)
+{
+  return stm32_eeprom_erase_write(addr, NULL, eraselen);
+}
+
+#endif /* defined(CONFIG_STM32_STM32L15XX) */
+
 #if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX)
 size_t up_progmem_pagesize(size_t page)
 {
@@ -260,6 +480,8 @@ size_t up_progmem_getaddress(size_t page)
 
 #endif /* def CONFIG_STM32_STM32F40XX */
 
+#if !defined(CONFIG_STM32_STM32L15XX)
+
 size_t up_progmem_npages(void)
 {
   return STM32_FLASH_NPAGES;
@@ -271,14 +493,14 @@ bool up_progmem_isuniform(void)
   return true;
 #else
   return false;
-#endif /* def STM32_FLASH_PAGESIZE */
+#endif
 }
 
 ssize_t up_progmem_erasepage(size_t page)
 {
 #if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX)
   size_t page_address;
-#endif /* defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX) */
+#endif
 
   if (page >= STM32_FLASH_NPAGES)
     {
@@ -438,5 +660,7 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
   return written;
 }
 
+#endif /* !defined(CONFIG_STM32_STM32L15XX) */
+
 #endif /* defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX) || \
-          defined (CONFIG_STM32_STM32F40XX) */
+          defined(CONFIG_STM32_STM32F40XX) || defined(CONFIG_STM32_STM32L15XX) */
diff --git a/arch/arm/src/stm32/stm32_flash.h b/arch/arm/src/stm32/stm32_flash.h
index 10c5cc189ed..6da1b0b7034 100644
--- a/arch/arm/src/stm32/stm32_flash.h
+++ b/arch/arm/src/stm32/stm32_flash.h
@@ -46,4 +46,60 @@
 #include "chip.h"
 #include "chip/stm32_flash.h"
 
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_eeprom_size
+ *
+ * Description:
+ *   Get EEPROM data memory size
+ *
+ * Returns:
+ *  Length of EEPROM memory region
+ *
+ ************************************************************************************/
+
+size_t stm32_eeprom_size(void);
+
+/************************************************************************************
+ * Name: stm32_eeprom_getaddress
+ *
+ * Description:
+ *   Get EEPROM data memory address
+ *
+ * Returns:
+ *  Address of EEPROM memory region
+ *
+ ************************************************************************************/
+
+size_t stm32_eeprom_getaddress(void);
+
+/************************************************************************************
+ * Name: stm32_eeprom_write
+ *
+ * Description:
+ *    Write buffer to EEPROM data memory address
+ *
+ * Returns:
+ *  Number of written bytes or error code.
+ *
+ ************************************************************************************/
+
+ssize_t stm32_eeprom_write(size_t addr, const void *buf, size_t buflen);
+
+/************************************************************************************
+ * Name: stm32_eeprom_erase
+ *
+ * Description:
+ *    Erase memory on EEPROM data memory address
+ *
+ * Returns:
+ *  Number of erased bytes or error code.
+ *
+ ************************************************************************************/
+
+ssize_t stm32_eeprom_erase(size_t addr, size_t eraselen);
+
 #endif /* __ARCH_ARM_SRC_STM32_STM32_FLASH_H */
diff --git a/drivers/sensors/lis2dh.c b/drivers/sensors/lis2dh.c
index 003baa390ae..b24aa782e36 100644
--- a/drivers/sensors/lis2dh.c
+++ b/drivers/sensors/lis2dh.c
@@ -129,7 +129,7 @@ static int            lis2dh_access(FAR struct lis2dh_dev_s *dev,
                         uint8_t subaddr, FAR uint8_t *buf, int length);
 static int            lis2dh_get_reading(FAR struct lis2dh_dev_s *dev,
                         FAR struct lis2dh_vector_s *res, bool force_read);
-static int            lis2dh_powerdown(FAR struct lis2dh_dev_s  dev);
+static int            lis2dh_powerdown(FAR struct lis2dh_dev_s *dev);
 static int            lis2dh_reboot(FAR struct lis2dh_dev_s *dev);
 static int            lis2dh_poll(FAR struct file *filep,
                         FAR struct pollfd *fds, bool setup);
@@ -326,6 +326,20 @@ static ssize_t lis2dh_read(FAR struct file *filep, FAR char *buffer,
       return -EINVAL;
     }
 
+  err = sem_wait(&priv->devsem);
+  if (err < 0)
+    {
+      return -EINTR;
+    }
+
+  /* Do not allow read() if no SNIOC_WRITESETUP first. */
+
+  if (!priv->setup)
+    {
+      lis2dh_dbg("lis2dh: Read from unconfigured device\n");
+      return -EINVAL;
+    }
+
   flags = enter_critical_section();
 #ifdef LIS2DH_COUNT_INTS
   if (priv->int_pending > 0)
@@ -342,13 +356,6 @@ static ssize_t lis2dh_read(FAR struct file *filep, FAR char *buffer,
   /* Set pointer to first measurement data */
 
   ptr = (FAR struct lis2dh_result *)buffer;
-
-  err = sem_wait(&priv->devsem);
-  if (err < 0)
-    {
-      return -EINTR;
-    }
-
   ptr->header.meas_count = 0;
 
   if (!priv->fifo_used)
@@ -421,9 +428,9 @@ static ssize_t lis2dh_read(FAR struct file *filep, FAR char *buffer,
 
           fifo_num_samples = (buf & ST_LIS2DH_FIFOSR_NUM_SAMP_MASK) + 1;
 
-          if (fifo_num_samples > readcount)
+          if (fifo_num_samples > (readcount - ptr->header.meas_count))
             {
-              fifo_num_samples = readcount;
+              fifo_num_samples = (readcount - ptr->header.meas_count);
             }
 
           ptr->header.meas_count +=
diff --git a/include/nuttx/net/sixlowpan.h b/include/nuttx/net/sixlowpan.h
index ebeb34933ac..9f051857e43 100644
--- a/include/nuttx/net/sixlowpan.h
+++ b/include/nuttx/net/sixlowpan.h
@@ -42,8 +42,8 @@
  *
  ****************************************************************************/
 
-#ifndef __INCLUDE_NUTTX_NET_SIXLOWOAN_H
-#define __INCLUDE_NUTTX_NET_SIXLOWOAN_H
+#ifndef __INCLUDE_NUTTX_NET_SIXLOWPAN_H
+#define __INCLUDE_NUTTX_NET_SIXLOWPAN_H
 
 /****************************************************************************
  * Included Files
@@ -57,6 +57,8 @@
 #include <nuttx/net/iob.h>
 #include <nuttx/net/netdev.h>
 
+#ifdef CONFIG_NET_6LOWPAN
+
 /****************************************************************************
  * Pre-processor Definitions
  ****************************************************************************/
@@ -465,4 +467,5 @@ struct ieee802154_driver_s
 
 int sixlowpan_input(FAR struct ieee802154_driver_s *ieee);
 
-#endif /* __INCLUDE_NUTTX_NET_SIXLOWOAN_H */
+#endif /* CONFIG_NET_6LOWPAN */
+#endif /* __INCLUDE_NUTTX_NET_SIXLOWPAN_H */
diff --git a/include/nuttx/sensors/lis2dh.h b/include/nuttx/sensors/lis2dh.h
index f9fdd839ca4..bede2795c23 100644
--- a/include/nuttx/sensors/lis2dh.h
+++ b/include/nuttx/sensors/lis2dh.h
@@ -109,8 +109,12 @@ extern "C"
 
 /* I1_AOI1 ENABLE for INT2 (-X------) */
 
-#define ST_LIS2DH_CR3_I1_AOI1_ENABLED   0x40   /* AOI1 interrupt on INT1 pin.*/
-#define ST_LIS2DH_CR3_I1_AOI2_ENABLED   0x20   /* AOI2 interrupt on INT1 pin.*/
+#define ST_LIS2DH_CR3_I1_AOI1_ENABLED   0x40   /* AOI1 interrupt on INT1 pin. */
+#define ST_LIS2DH_CR3_I1_AOI2_ENABLED   0x20   /* AOI2 interrupt on INT1 pin. */
+#define ST_LIS2DH_CR3_I1_DRDY1          0x10   /* DRDY1 interrupt on INT1 pin. */
+#define ST_LIS2DH_CR3_I1_DRDY2          0x08   /* DRDY2 interrupt on INT1 pin. */
+#define ST_LIS2DH_CR3_I1_WTM            0x04   /* FIFO Watermark interrupt on INT1 pin. */
+#define ST_LIS2DH_CR3_I1_OVERRUN        0x02   /* FIFO Overrun interrupt on INT1 pin. */
 
 #define ST_LIS2DH_CTRL_REG4             0x23
 
diff --git a/net/Kconfig b/net/Kconfig
index 21ef8b3e7a5..da5eab15d2c 100644
--- a/net/Kconfig
+++ b/net/Kconfig
@@ -131,7 +131,7 @@ config NET_ETHERNET
 		no need to define anything special in the configuration file to use
 		Ethernet -- it is the default).
 
-menuconfig NET_6LOWPAN
+config NET_6LOWPAN
 	bool "IEEE 802.15.4 6LoWPAN support"
 	default n
 	select NETDEV_MULTINIC if NET_ETHERNET || NET_LOOPBACK || NET_SLIP || NET_TUN
diff --git a/net/sixlowpan/Kconfig b/net/sixlowpan/Kconfig
index 49f4756a024..d7635cf3625 100644
--- a/net/sixlowpan/Kconfig
+++ b/net/sixlowpan/Kconfig
@@ -3,7 +3,8 @@
 # see the file kconfig-language.txt in the NuttX tools repository.
 #
 
-if NET_6LOWPAN
+menu "6LoWPAN Configuration"
+	depends on NET_6LOWPAN
 
 config NET_6LOWPAN_FRAG
 	bool "6loWPAN Fragmentation"
@@ -51,6 +52,17 @@ config NET_6LOWPAN_COMPRESSION_THRESHOLD
 		CONFIG_NET_6LOWPAN_COMPRESSION_THRESHOLD sets a lower threshold for
 		when packets should not be compressed.
 
+config NET_6LOWPAN_MINPORT
+	hex "Minimum port nubmer"
+	default 0x5471
+	depends on NET_6LOWPAN_COMPRESSION_HC1
+	---help---
+		HC1 compression of UDP headersis feasible only if both src and dest
+		ports are between CONFIG_NET_6LOWPAN_MINPORT and
+		CONFIG_NET_6LOWPAN_MINPORT + 15, inclusive.
+
+		All nodes must agree on the value of CONFIG_NET_6LOWPAN_MINPORT
+
 if NET_6LOWPAN_COMPRESSION_HC06
 
 config NET_6LOWPAN_MAXADDRCONTEXT
@@ -179,4 +191,13 @@ config NET_6LOWPAN_TCP_RECVWNDO
 		the application is slow to process incoming data, or high (32768
 		bytes) if the application processes data quickly.
 
-endif # NET_6LOWPAN
+config NET_6LOWPAN_DUMPBUFFER
+	bool "Enable dumping of buffer data"
+	default n
+	depends on DEBUG_NET_INFO
+	---help---
+		Enable dumping of all packet and frame buffers coming into and out
+		of the 6loWPAN logic.  This will generate a large volume of data if
+		selected.
+
+endmenu # 6LoWPAN Configuration
diff --git a/net/sixlowpan/sixlowpan_framelist.c b/net/sixlowpan/sixlowpan_framelist.c
index f206a3fe9fd..58f4ba624e0 100644
--- a/net/sixlowpan/sixlowpan_framelist.c
+++ b/net/sixlowpan/sixlowpan_framelist.c
@@ -102,6 +102,7 @@
  * Input Parameters:
  *   ieee   - Pointer to IEEE802.15.4 MAC driver structure.
  *   destip - Pointer to the IPv6 header to "compress"
+ *   fptr   - Pointer to the beginning of the frame under construction
  *
  * Returned Value:
  *   None
@@ -109,18 +110,19 @@
  ****************************************************************************/
 
 static void sixlowpan_compress_ipv6hdr(FAR struct ieee802154_driver_s *ieee,
-                                       FAR const struct ipv6_hdr_s *destip)
+                                       FAR const struct ipv6_hdr_s *destip,
+                                       FAR uint8_t *fptr)
 {
   /* Indicate the IPv6 dispatch and length */
 
-  *g_rimeptr       = SIXLOWPAN_DISPATCH_IPV6;
-  g_rime_hdrlen   += SIXLOWPAN_IPV6_HDR_LEN;
+  fptr[g_frame_hdrlen] = SIXLOWPAN_DISPATCH_IPV6;
+  g_frame_hdrlen      += SIXLOWPAN_IPV6_HDR_LEN;
 
   /* Copy the IPv6 header and adjust pointers */
 
-  memcpy(g_rimeptr + g_rime_hdrlen, destip, IPv6_HDRLEN);
-  g_rime_hdrlen   += IPv6_HDRLEN;
-  g_uncomp_hdrlen += IPv6_HDRLEN;
+  memcpy(&fptr[g_frame_hdrlen] , destip, IPv6_HDRLEN);
+  g_frame_hdrlen      += IPv6_HDRLEN;
+  g_uncomp_hdrlen     += IPv6_HDRLEN;
 }
 
 /****************************************************************************
@@ -132,10 +134,10 @@ static void sixlowpan_compress_ipv6hdr(FAR struct ieee802154_driver_s *ieee,
  *
  * Description:
  *   Process an outgoing UDP or TCP packet.  This function is called from
- *   send interrupt logic when a TX poll is received.  It formates the
+ *   send interrupt logic when a TX poll is received.  It formats the
  *   list of frames to be sent by the IEEE802.15.4 MAC driver.
  *
- *   The payload data is in the caller 's_buf' and is of length 's_len'.
+ *   The payload data is in the caller 'buf' and is of length 'buflen'.
  *   Compressed headers will be added and if necessary the packet is
  *   fragmented. The resulting packet/fragments are put in ieee->i_framelist
  *   and the entire list of frames will be delivered to the 802.15.4 MAC via
@@ -144,8 +146,9 @@ static void sixlowpan_compress_ipv6hdr(FAR struct ieee802154_driver_s *ieee,
  * Input Parameters:
  *   ieee    - The IEEE802.15.4 MAC driver instance
  *   ipv6hdr - IPv6 header followed by TCP or UDP header.
- *   buf     - Data to send
- *   len     - Length of data to send
+ *   buf     - Beginning of the packet packet to send (with IPv6 + protocol
+ *             headers)
+ *   buflen  - Length of data to send (include IPv6 and protocol headers)
  *   destmac - The IEEE802.15.4 MAC address of the destination
  *
  * Returned Value:
@@ -161,10 +164,11 @@ static void sixlowpan_compress_ipv6hdr(FAR struct ieee802154_driver_s *ieee,
 
 int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
                            FAR const struct ipv6_hdr_s *destip,
-                           FAR const void *buf, size_t len,
+                           FAR const void *buf, size_t buflen,
                            FAR const struct rimeaddr_s *destmac)
 {
   FAR struct iob_s *iob;
+  FAR uint8_t *fptr;
   int framer_hdrlen;
   struct rimeaddr_s bcastmac;
   uint16_t outlen = 0;
@@ -175,9 +179,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
 
   FRAME_RESET();
   g_uncomp_hdrlen = 0;
-  g_rime_hdrlen   = 0;
-  /* REVISIT: Do I need this rimeptr? */
-  g_rimeptr       = &ieee->i_dev.d_buf[PACKETBUF_HDR_SIZE];
+  g_frame_hdrlen  = 0;
 
   /* Reset rime buffer, packet buffer metatadata */
 
@@ -228,18 +230,32 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
   iob->io_len    = 0;
   iob->io_offset = 0;
   iob->io_pktlen = 0;
+  fptr           = iob->io_data;
 
-  ninfo("Sending packet len %d\n", len);
+  ninfo("Sending packet length %d\n", buflen);
+
+  /* Pre-calculate frame header length. */
+
+  framer_hdrlen = sixlowpan_send_hdrlen(ieee, ieee->i_panid);
+  if (framer_hdrlen < 0)
+    {
+      /* Failed to determine the size of the header failed. */
+
+      nerr("ERROR: sixlowpan_send_hdrlen() failed: %d\n", framer_hdrlen);
+      return framer_hdrlen;
+    }
+
+  g_frame_hdrlen  = framer_hdrlen;
 
 #ifndef CONFIG_NET_6LOWPAN_COMPRESSION_IPv6
-  if (len >= CONFIG_NET_6LOWPAN_COMPRESSION_THRESHOLD)
+  if (buflen >= CONFIG_NET_6LOWPAN_COMPRESSION_THRESHOLD)
     {
       /* Try to compress the headers */
 
 #if defined(CONFIG_NET_6LOWPAN_COMPRESSION_HC1)
-      sixlowpan_compresshdr_hc1(ieee, destip, destmac, iob);
+      sixlowpan_compresshdr_hc1(ieee, destip, destmac, fptr);
 #elif defined(CONFIG_NET_6LOWPAN_COMPRESSION_HC06)
-      sixlowpan_compresshdr_hc06(ieee, destip, destmac, iob);
+      sixlowpan_compresshdr_hc06(ieee, destip, destmac, fptr);
 #else
 #  error No compression specified
 #endif
@@ -249,29 +265,18 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
     {
       /* Small.. use IPv6 dispatch (no compression) */
 
-      sixlowpan_compress_ipv6hdr(ieee, destip);
+      sixlowpan_compress_ipv6hdr(ieee, destip, fptr);
     }
 
-  ninfo("Header of len %d\n", g_rime_hdrlen);
+  ninfo("Header of length %d\n", g_frame_hdrlen);
 
   rimeaddr_copy(&g_pktaddrs[PACKETBUF_ADDR_RECEIVER], destmac);
 
-  /* Pre-calculate frame header length. */
-
-  framer_hdrlen = sixlowpan_hdrlen(ieee, ieee->i_panid);
-  if (framer_hdrlen < 0)
-    {
-      /* Failed to determine the size of the header failed. */
-
-      nerr("ERROR: sixlowpan_hdrlen() failed: %d\n", framer_hdrlen);
-      return framer_hdrlen;
-    }
-
   /* Check if we need to fragment the packet into several frames */
 
-  if ((int)len - (int)g_uncomp_hdrlen >
+  if ((int)buflen - (int)g_uncomp_hdrlen >
       (int)CONFIG_NET_6LOWPAN_MAXPAYLOAD - framer_hdrlen -
-      (int)g_rime_hdrlen)
+      (int)g_frame_hdrlen)
     {
 #if CONFIG_NET_6LOWPAN_FRAG
       /* ieee->i_framelist will hold the generated frames; frames will be
@@ -288,7 +293,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
        * The following fragments contain only the fragn dispatch.
        */
 
-      ninfo("Fragmentation sending packet len %d\n", len);
+      ninfo("Fragmentation sending packet length %d\n", buflen);
 
       /* Create 1st Fragment */
       /* Add the frame header using the pre-allocated IOB. */
@@ -299,7 +304,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
 
       /* Move HC1/HC06/IPv6 header */
 
-      memmove(g_rimeptr + SIXLOWPAN_FRAG1_HDR_LEN, g_rimeptr, g_rime_hdrlen);
+      memmove(fptr + SIXLOWPAN_FRAG1_HDR_LEN, fptr, g_frame_hdrlen);
 
       /* Setup up the fragment header.
        *
@@ -316,27 +321,29 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
        * bytes for all subsequent headers.
        */
 
-      PUTINT16(RIME_FRAG_PTR, RIME_FRAG_DISPATCH_SIZE,
-              ((SIXLOWPAN_DISPATCH_FRAG1 << 8) | len));
-      PUTINT16(RIME_FRAG_PTR, RIME_FRAG_TAG, ieee->i_dgramtag);
+      PUTINT16(fptr, RIME_FRAG_DISPATCH_SIZE,
+              ((SIXLOWPAN_DISPATCH_FRAG1 << 8) | buflen));
+      PUTINT16(fptr, RIME_FRAG_TAG, ieee->i_dgramtag);
       ieee->i_dgramtag++;
 
       /* Copy payload and enqueue */
 
-      g_rime_hdrlen += SIXLOWPAN_FRAG1_HDR_LEN;
+      g_frame_hdrlen   += SIXLOWPAN_FRAG1_HDR_LEN;
       g_rime_payloadlen =
-        (CONFIG_NET_6LOWPAN_MAXPAYLOAD - framer_hdrlen - g_rime_hdrlen) & 0xf8;
+        (CONFIG_NET_6LOWPAN_MAXPAYLOAD - framer_hdrlen - g_frame_hdrlen) & 0xf8;
 
-      memcpy(g_rimeptr + g_rime_hdrlen,
+      memcpy(fptr + g_frame_hdrlen,
              (FAR uint8_t *)destip + g_uncomp_hdrlen, g_rime_payloadlen);
-      iob->io_len += g_rime_payloadlen + g_rime_hdrlen;
+      iob->io_len += g_rime_payloadlen + g_frame_hdrlen;
 
       /* Set outlen to what we already sent from the IP payload */
 
       outlen = g_rime_payloadlen + g_uncomp_hdrlen;
 
-      ninfo("First fragment: len %d, tag %d\n",
+      ninfo("First fragment: length %d, tag %d\n",
             g_rime_payloadlen, ieee->i_dgramtag);
+      sixlowpan_dumpbuffer("Outgoing frame",
+                           (FAR const uint8_t *)iob->io_data, iob->io_len);
 
       /* Add the first frame to the IOB queue */
 
@@ -349,9 +356,9 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
 
       /* Create following fragments */
 
-      g_rime_hdrlen = SIXLOWPAN_FRAGN_HDR_LEN;
+      g_frame_hdrlen = SIXLOWPAN_FRAGN_HDR_LEN;
 
-      while (outlen < len)
+      while (outlen < buflen)
         {
           /* Allocate an IOB to hold the next fragment, waiting if
            * necessary.
@@ -366,6 +373,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
           iob->io_len    = 0;
           iob->io_offset = 0;
           iob->io_pktlen = 0;
+          fptr           = iob->io_data;
 
           /* Add the frame header */
 
@@ -375,39 +383,42 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
 
           /* Move HC1/HC06/IPv6 header */
 
-          memmove(g_rimeptr + SIXLOWPAN_FRAGN_HDR_LEN, g_rimeptr, g_rime_hdrlen);
+          memmove(fptr + SIXLOWPAN_FRAGN_HDR_LEN, fptr, g_frame_hdrlen);
 
           /* Setup up the fragment header */
 
-          PUTINT16(RIME_FRAG_PTR, RIME_FRAG_DISPATCH_SIZE,
-                  ((SIXLOWPAN_DISPATCH_FRAGN << 8) | len));
-          PUTINT16(RIME_FRAG_PTR, RIME_FRAG_TAG, ieee->i_dgramtag);
-          RIME_FRAG_PTR[RIME_FRAG_OFFSET] = outlen >> 3;
+          PUTINT16(fptr, RIME_FRAG_DISPATCH_SIZE,
+                  ((SIXLOWPAN_DISPATCH_FRAGN << 8) | buflen));
+          PUTINT16(fptr, RIME_FRAG_TAG, ieee->i_dgramtag);
+          fptr[RIME_FRAG_OFFSET] = outlen >> 3;
 
           /* Copy payload and enqueue */
 
-          if (len - outlen < g_rime_payloadlen)
+          if (buflen - outlen < g_rime_payloadlen)
             {
               /* Last fragment */
 
-              g_rime_payloadlen = len - outlen;
+              g_rime_payloadlen = buflen - outlen;
             }
           else
             {
               g_rime_payloadlen =
-                (CONFIG_NET_6LOWPAN_MAXPAYLOAD - framer_hdrlen - g_rime_hdrlen) & 0xf8;
+                (CONFIG_NET_6LOWPAN_MAXPAYLOAD - framer_hdrlen - g_frame_hdrlen) & 0xf8;
             }
 
-          memcpy(g_rimeptr + g_rime_hdrlen, (FAR uint8_t *)destip + outlen,
+          memcpy(fptr + g_frame_hdrlen, (FAR uint8_t *)destip + outlen,
                  g_rime_payloadlen);
-          iob->io_len = g_rime_payloadlen + g_rime_hdrlen;
+          iob->io_len = g_rime_payloadlen + g_frame_hdrlen;
 
           /* Set outlen to what we already sent from the IP payload */
 
           outlen += (g_rime_payloadlen + g_uncomp_hdrlen);
 
-          ninfo("sixlowpan output: fragment offset %d, len %d, tag %d\n",
+          ninfo("sixlowpan output: fragment offset %d, length %d, tag %d\n",
                 outlen >> 3, g_rime_payloadlen, ieee->i_dgramtag);
+          sixlowpan_dumpbuffer("Outgoing frame",
+                               (FAR const uint8_t *)iob->io_data,
+                               iob->io_len);
 
           /* Add the next frame to the tail of the IOB queue */
 
@@ -418,7 +429,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
           ieee->i_framelist->io_pktlen += iob->io_len;
         }
 #else
-      nerr("ERROR: Packet too large: %d\n", len);
+      nerr("ERROR: Packet too large: %d\n", buflen);
       nerr("       Cannot to be sent without fragmentation support\n");
       nerr("       dropping packet\n");
 
@@ -433,7 +444,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
        * and send in one frame.
        */
 
-      /* Add the frame header to the prealloated IOB. */
+      /* Add the frame header to the preallocated IOB. */
 
       verify = sixlowpan_framecreate(ieee, iob, ieee->i_panid);
       DEBUGASSERT(verify == framer_hdrlen);
@@ -441,9 +452,13 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee,
 
       /* Copy the payload and queue */
 
-      memcpy(g_rimeptr + g_rime_hdrlen, (FAR uint8_t *)destip + g_uncomp_hdrlen,
-             len - g_uncomp_hdrlen);
-      iob->io_len = len - g_uncomp_hdrlen + g_rime_hdrlen;
+      memcpy(fptr + g_frame_hdrlen, (FAR uint8_t *)destip + g_uncomp_hdrlen,
+             buflen - g_uncomp_hdrlen);
+      iob->io_len = buflen - g_uncomp_hdrlen + g_frame_hdrlen;
+
+      ninfo("Non-fragmented: length %d\n", iob->io_len);
+      sixlowpan_dumpbuffer("Outgoing frame",
+                       (FAR const uint8_t *)iob->io_data, iob->io_len);
 
       /* Add the first frame to the IOB queue */
 
diff --git a/net/sixlowpan/sixlowpan_framer.c b/net/sixlowpan/sixlowpan_framer.c
index d0efa291d41..1adad943abb 100644
--- a/net/sixlowpan/sixlowpan_framer.c
+++ b/net/sixlowpan/sixlowpan_framer.c
@@ -294,7 +294,7 @@ static void sixlowpan_setup_params(FAR struct ieee802154_driver_s *ieee,
 
   /* Initialize all prameters to all zero */
 
-  memset(&params, 0, sizeof(params));
+  memset(params, 0, sizeof(params));
 
   /* Reset to an empty frame */
 
@@ -383,7 +383,7 @@ static void sixlowpan_setup_params(FAR struct ieee802154_driver_s *ieee,
  ****************************************************************************/
 
 /****************************************************************************
- * Function: sixlowpan_hdrlen
+ * Function: sixlowpan_send_hdrlen
  *
  * Description:
  *   This function is before the first frame has been sent in order to
@@ -401,7 +401,7 @@ static void sixlowpan_setup_params(FAR struct ieee802154_driver_s *ieee,
  *
  ****************************************************************************/
 
-int sixlowpan_hdrlen(FAR struct ieee802154_driver_s *ieee,
+int sixlowpan_send_hdrlen(FAR struct ieee802154_driver_s *ieee,
                      uint16_t dest_panid)
 {
   struct frame802154_s params;
@@ -453,18 +453,18 @@ int sixlowpan_802154_framecreate(FAR struct frame802154_s *finfo,
       return 0;
     }
 
-  /* OK, now we have field lengths.  Time to actually construct
-   * the outgoing frame, and store it in the provided buffer
+  /* OK, now we have field lengths.  Time to actually construct the outgoing
+   * frame, and store it in the provided buffer
    */
 
-  buf[0] = (finfo->fcf.frame_type         & 7) |
-           ((finfo->fcf.security_enabled  & 1) << 3) |
-           ((finfo->fcf.frame_pending     & 1) << 4) |
-           ((finfo->fcf.ack_required      & 1) << 5) |
-           ((finfo->fcf.panid_compression & 1) << 6);
-  buf[1] = ((finfo->fcf.dest_addr_mode    & 3) << 2) |
-           ((finfo->fcf.frame_version     & 3) << 4) |
-           ((finfo->fcf.src_addr_mode     & 3) << 6);
+  buf[0] = ((finfo->fcf.frame_type        & 7) << FRAME802154_FRAMETYPE_SHIFT)    |
+           ((finfo->fcf.security_enabled  & 1) << FRAME802154_SECENABLED_SHIFT)   |
+           ((finfo->fcf.frame_pending     & 1) << FRAME802154_FRAMEPENDING_SHIFT) |
+           ((finfo->fcf.ack_required      & 1) << FRAME802154_ACKREQUEST_SHIFT)   |
+           ((finfo->fcf.panid_compression & 1) << FRAME802154_PANIDCOMP_SHIFT);
+  buf[1] = ((finfo->fcf.dest_addr_mode    & 3) << FRAME802154_DSTADDR_SHIFT)      |
+           ((finfo->fcf.frame_version     & 3) << FRAME802154_VERSION_SHIFT)      |
+           ((finfo->fcf.src_addr_mode     & 3) << FRAME802154_SRCADDR_SHIFT);
 
   /* Sequence number */
 
@@ -483,7 +483,7 @@ int sixlowpan_802154_framecreate(FAR struct frame802154_s *finfo,
 
   for (i = flen.dest_addr_len; i > 0; i--)
     {
-     buf[pos++] = finfo->dest_addr[i - 1];
+      buf[pos++] = finfo->dest_addr[i - 1];
     }
 
   /* Source PAN ID */
diff --git a/net/sixlowpan/sixlowpan_globals.c b/net/sixlowpan/sixlowpan_globals.c
index 2f9843022ff..274ef47fa77 100644
--- a/net/sixlowpan/sixlowpan_globals.c
+++ b/net/sixlowpan/sixlowpan_globals.c
@@ -54,14 +54,6 @@
  * during that processing
  */
 
-/* A pointer to the rime buffer.
- *
- * We initialize it to the beginning of the rime buffer, then access
- * different fields by updating the offset ieee->g_rime_hdrlen.
- */
-
-FAR uint8_t *g_rimeptr;
-
 /* The length of the payload in the Rime buffer.
  *
  * The payload is what comes after the compressed or uncompressed headers
@@ -77,12 +69,12 @@ uint8_t g_rime_payloadlen;
 
 uint8_t g_uncomp_hdrlen;
 
-/* g_rime_hdrlen is the total length of (the processed) 6lowpan headers
+/* g_frame_hdrlen is the total length of (the processed) 6lowpan headers
  * (fragment headers, IPV6 or HC1, HC2, and HC1 and HC2 non compressed
  * fields).
  */
 
-uint8_t g_rime_hdrlen;
+uint8_t g_frame_hdrlen;
 
 /* Offset first available byte for the payload after header region. */
 
diff --git a/net/sixlowpan/sixlowpan_hc06.c b/net/sixlowpan/sixlowpan_hc06.c
index 5caee4b8383..f5d8396e911 100644
--- a/net/sixlowpan/sixlowpan_hc06.c
+++ b/net/sixlowpan/sixlowpan_hc06.c
@@ -437,7 +437,7 @@ void sixlowpan_hc06_initialize(void)
  *   ipv6    - The IPv6 header to be compressed
  *   destmac - L2 destination address, needed to compress the IP
  *             destination field
- *   iob     - The IOB into which the compressed header should be saved.
+ *   fptr    - Pointer to frame data payload.
  *
  * Returned Value:
  *   None
@@ -447,18 +447,19 @@ void sixlowpan_hc06_initialize(void)
 void sixlowpan_compresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
                                 FAR const struct ipv6_hdr_s *ipv6,
                                 FAR const struct rimeaddr_s *destmac,
-                                FAR struct iob_s *iob)
+                                FAR uint8_t *fptr)
 {
-  FAR uint8_t *iphc = RIME_IPHC_BUF;
+  FAR uint8_t *iphc = fptr + g_frame_hdrlen;
   FAR struct sixlowpan_addrcontext_s *addrcontext;
   uint8_t iphc0;
   uint8_t iphc1;
   uint8_t tmp;
 
-  ninfodumpbuffer("IPv6 before compression", (FAR const uint8_t *)ipv6,
-                  sizeof(struct ipv6_hdr_s));
+  sixlowpan_dumpbuffer("IPv6 before compression",
+                       (FAR const uint8_t *)ipv6,
+                       sizeof(struct ipv6_hdr_s));
 
-  g_hc06ptr = g_rimeptr + 2;
+  g_hc06ptr = fptr + 2;
 
   /* As we copy some bit-length fields, in the IPHC encoding bytes,
    * we sometimes use |=
@@ -803,12 +804,12 @@ void sixlowpan_compresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
     }
 #endif /* CONFIG_NET_UDP */
 
-  /* Before the g_rime_hdrlen operation */
+  /* Before the g_frame_hdrlen operation */
 
   iphc[0] = iphc0;
   iphc[1] = iphc1;
 
-  g_rime_hdrlen = g_hc06ptr - g_rimeptr;
+  g_frame_hdrlen = g_hc06ptr - fptr;
   return;
 }
 
@@ -822,14 +823,16 @@ void sixlowpan_compresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
  *   This function is called by the input function when the dispatch is HC06.
  *   We process the packet in the rime buffer, uncompress the header fields,
  *   and copy the result in the sixlowpan buffer.  At the end of the
- *   decompression, g_rime_hdrlen and g_uncompressed_hdrlen are set to the
+ *   decompression, g_frame_hdrlen and g_uncompressed_hdrlen are set to the
  *   appropriate values
  *
  * Input Parmeters:
- *   ieee  - A reference to the IEE802.15.4 network device state
- *   iplen - Equal to 0 if the packet is not a fragment (IP length is then
- *           inferred from the L2 length), non 0 if the packet is a first
- *           fragment.
+ *   ieee   - A reference to the IEE802.15.4 network device state
+ *   iplen  - Equal to 0 if the packet is not a fragment (IP length is then
+ *            inferred from the L2 length), non 0 if the packet is a first
+ *            fragment.
+ *   iob    - Pointer to the IOB containing the received frame.
+ *   payptr - Pointer to the frame data payload.
  *
  * Returned Value:
  *   None
@@ -837,17 +840,18 @@ void sixlowpan_compresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
  ****************************************************************************/
 
 void sixlowpan_uncompresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
-                                  uint16_t iplen)
+                                  uint16_t iplen, FAR struct iob_s *iob,
+                                  FAR char *payptr)
 {
   FAR struct ipv6_hdr_s *ipv6 = IPv6BUF(ieee);
-  FAR uint8_t *iphc = RIME_IPHC_BUF;
+  FAR uint8_t *iphc = payptr + g_frame_hdrlen;
   uint8_t iphc0;
   uint8_t iphc1;
   uint8_t tmp;
 
   /* At least two byte will be used for the encoding */
 
-  g_hc06ptr = g_rimeptr + g_rime_hdrlen + 2;
+  g_hc06ptr = payptr + g_frame_hdrlen + 2;
 
   iphc0 = iphc[0];
   iphc1 = iphc[1];
@@ -1153,7 +1157,7 @@ void sixlowpan_uncompresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
         }
     }
 
-  g_rime_hdrlen = g_hc06ptr - g_rimeptr;
+  g_frame_hdrlen = g_hc06ptr - payptr;
 
   /* IP length field. */
 
@@ -1162,7 +1166,8 @@ void sixlowpan_uncompresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
       /* This is not a fragmented packet */
 
       ipv6->len[0] = 0;
-      ipv6->len[1] = ieee->i_dev.d_len - g_rime_hdrlen + g_uncomp_hdrlen - IPv6_HDRLEN;
+      ipv6->len[1] = iob->io_len - g_frame_hdrlen + g_uncomp_hdrlen -
+                     IPv6_HDRLEN;
     }
   else
     {
diff --git a/net/sixlowpan/sixlowpan_hc1.c b/net/sixlowpan/sixlowpan_hc1.c
index fd45280ea04..a7040d5bcb3 100644
--- a/net/sixlowpan/sixlowpan_hc1.c
+++ b/net/sixlowpan/sixlowpan_hc1.c
@@ -48,6 +48,10 @@
 
 #include <nuttx/config.h>
 
+#include <string.h>
+#include <errno.h>
+#include <debug.h>
+
 #include <nuttx/net/netdev.h>
 #include "sixlowpan/sixlowpan_internal.h"
 
@@ -59,10 +63,8 @@
 
 /* Buffer access helpers */
 
-#define IPv6BUF(dev)  \
-  ((FAR struct ipv6_hdr_s *)((dev)->d_buf))
-#define UDPIPv6BUF(dev) \
-  ((FAR struct udp_hdr_s *)&dev->d_buf[NET_LL_HDRLEN(dev) + IPv6_HDRLEN])
+#define IPv6BUF(dev)    ((FAR struct ipv6_hdr_s *)((dev)->d_buf))
+#define UDPIPv6BUF(dev) ((FAR struct udp_hdr_s *)&(dev)->d_buf[IPv6_HDRLEN])
 
 /****************************************************************************
  * Public Functions
@@ -117,7 +119,7 @@
  *   ipv6    - The IPv6 header to be compressed
  *   destmac - L2 destination address, needed to compress the IP
  *             destination field
- *   iob     - The IOB into which the compressed header should be saved.
+ *   fptr    - Pointer to frame to be compressed.
  *
  * Returned Value:
  *   None
@@ -127,30 +129,33 @@
 void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
                                FAR const struct ipv6_hdr_s *ipv6,
                                FAR const struct rimeaddr_s *destmac,
-                               FAR struct iob_s *iob)
+                               FAR uint8_t *fptr)
 {
-  FAR uint8_t *hc1 = RIME_HC1_PTR;
+  FAR uint8_t *hc1 = fptr + g_frame_hdrlen;
 
   /* Check if all the assumptions for full compression are valid */
 
-  if (ipv6->vtc != 0x60 || ipv6->tcflow != 0 || ipv6->flow != 0 ||
-      !sixlowpan_islinklocal(&ipv6->srcipaddr) ||
-      !sixlowpan_ismacbased(&ipv6->srcipaddr, &ieee->i_rimeaddr) ||
-      !sixlowpan_islinklocal(&ipv6->destipaddr) ||
-      !sixlowpan_ismacbased(&ipv6->destipaddr, destmac) ||
+  if (ipv6->vtc != 0x60 || ipv6->tcf != 0 || ipv6->flow != 0 ||
+      !sixlowpan_islinklocal(ipv6->srcipaddr) ||
+      !sixlowpan_ismacbased(ipv6->srcipaddr, &ieee->i_nodeaddr) ||
+      !sixlowpan_islinklocal(ipv6->destipaddr) ||
+      !sixlowpan_ismacbased(ipv6->destipaddr, destmac) ||
       (ipv6->proto != IP_PROTO_ICMP6 && ipv6->proto != IP_PROTO_UDP &&
        ipv6->proto != IP_PROTO_TCP))
     {
       /* IPV6 DISPATCH
-       * Something cannot be compressed, use IPV6 DISPATCH,
-       * compress nothing, copy IPv6 header in rime buffer
+       * Something cannot be compressed, use IPV6 DISPATCH, compress
+       * nothing, copy IPv6 header in rime buffer
        */
 
-      *g_rimeptr      = SIXLOWPAN_DISPATCH_IPV6;
-       g_rime_hdrlen += SIXLOWPAN_IPV6_HDR_LEN;
-       memcpy(g_rimeptr + g_rime_hdrlen, ipv6, IPv6_HDRLEN);
-       g_rime_hdrlen += IPv6_HDRLEN;
-       g_uncomp_hdrlen += IPv6_HDRLEN;
+      /* IPv6 dispatch header (1 byte) */
+
+      hc1[RIME_HC1_DISPATCH] = SIXLOWPAN_DISPATCH_IPV6;
+      g_frame_hdrlen        += SIXLOWPAN_IPV6_HDR_LEN;
+
+      memcpy(fptr + g_frame_hdrlen, ipv6, IPv6_HDRLEN);
+      g_frame_hdrlen        += IPv6_HDRLEN;
+      g_uncomp_hdrlen       += IPv6_HDRLEN;
     }
   else
     {
@@ -167,8 +172,8 @@ void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
           /* HC1 encoding and ttl */
 
           hc1[RIME_HC1_ENCODING] = 0xfc;
-          hc1[RIME_HC1_TTL] = ipv6->ttl;
-          g_rime_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
+          hc1[RIME_HC1_TTL]  = ipv6->ttl;
+          g_frame_hdrlen    += SIXLOWPAN_HC1_HDR_LEN;
           break;
 
 #if CONFIG_NET_TCP
@@ -176,54 +181,57 @@ void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
           /* HC1 encoding and ttl */
 
           hc1[RIME_HC1_ENCODING] = 0xfe;
-          hc1[RIME_HC1_TTL] = ipv6->ttl;
-          g_rime_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
+          hc1[RIME_HC1_TTL]  = ipv6->ttl;
+          g_frame_hdrlen    += SIXLOWPAN_HC1_HDR_LEN;
           break;
 #endif /* CONFIG_NET_TCP */
 
 #if CONFIG_NET_UDP
         case IP_PROTO_UDP:
-          FAR struct udp_hdr_s *udp = UDPIPv6BUF(dev);
+          {
+            FAR struct udp_hdr_s *udp = UDPIPv6BUF(&ieee->i_dev);
 
-          /* Try to compress UDP header (we do only full compression).  This
-           * is feasible if both src and dest ports are between
-           * SIXLOWPAN_UDP_PORT_MIN and SIXLOWPAN_UDP_PORT_MIN + 15
-           */
+            /* Try to compress UDP header (we do only full compression).
+             * This is feasible if both src and dest ports are between
+             * CONFIG_NET_6LOWPAN_MINPORT and CONFIG_NET_6LOWPAN_MINPORT +
+             * 15
+             */
 
-          ninfo("local/remote port %u/%u\n", udp->srcport, udp->destport);
+            ninfo("local/remote port %u/%u\n", udp->srcport, udp->destport);
 
-          if (htons(udp->srcport)  >= SIXLOWPAN_UDP_PORT_MIN &&
-              htons(udp->srcport)  <  SIXLOWPAN_UDP_PORT_MAX &&
-              htons(udp->destport) >= SIXLOWPAN_UDP_PORT_MIN &&
-              htons(udp->destport) <  SIXLOWPAN_UDP_PORT_MAX)
-            {
-              FAR uint8_t *hcudp = RIME_HC1_HC_UDP_PTR;
+            if (htons(udp->srcport)  >=  CONFIG_NET_6LOWPAN_MINPORT &&
+                htons(udp->srcport)  <  (CONFIG_NET_6LOWPAN_MINPORT + 16) &&
+                htons(udp->destport) >=  CONFIG_NET_6LOWPAN_MINPORT &&
+                htons(udp->destport) <  (CONFIG_NET_6LOWPAN_MINPORT + 16))
+              {
+                FAR uint8_t *hcudp = fptr + g_frame_hdrlen;
 
-              /* HC1 encoding */
+                /* HC1 encoding */
 
-              hcudp[RIME_HC1_HC_UDP_HC1_ENCODING] = 0xfb;
+                hcudp[RIME_HC1_HC_UDP_HC1_ENCODING] = 0xfb;
 
-              /* HC_UDP encoding, ttl, src and dest ports, checksum */
+                /* HC_UDP encoding, ttl, src and dest ports, checksum */
 
-              hcudp[RIME_HC1_HC_UDP_UDP_ENCODING] = 0xe0;
-              hcudp[RIME_HC1_HC_UDP_TTL]          = ipv6->ttl;
-              hcudp[RIME_HC1_HC_UDP_PORTS]        =
-                (uint8_t)((htons(udp->srcport) - SIXLOWPAN_UDP_PORT_MIN) << 4) +
-                (uint8_t)((htons(udp->destport) - SIXLOWPAN_UDP_PORT_MIN));
+                hcudp[RIME_HC1_HC_UDP_UDP_ENCODING] = 0xe0;
+                hcudp[RIME_HC1_HC_UDP_TTL]          = ipv6->ttl;
+                hcudp[RIME_HC1_HC_UDP_PORTS]        =
+                  (uint8_t)((htons(udp->srcport) - CONFIG_NET_6LOWPAN_MINPORT) << 4) +
+                  (uint8_t)((htons(udp->destport) - CONFIG_NET_6LOWPAN_MINPORT));
 
-              memcpy(&hcudp[RIME_HC1_HC_UDP_CHKSUM], &udp->udpchksum, 2);
+                memcpy(&hcudp[RIME_HC1_HC_UDP_CHKSUM], &udp->udpchksum, 2);
 
-              g_rime_hdrlen   += SIXLOWPAN_HC1_HC_UDP_HDR_LEN;
-              g_uncomp_hdrlen += UIP_UDPH_LEN;
-            }
-          else
-            {
-              /* HC1 encoding and ttl */
+                g_frame_hdrlen        += SIXLOWPAN_HC1_HC_UDP_HDR_LEN;
+                g_uncomp_hdrlen       += UDP_HDRLEN;
+              }
+            else
+              {
+                /* HC1 encoding and ttl */
 
-              hc1[RIME_HC1_ENCODING] = 0xfa;
-              hc1[RIME_HC1_TTL]      = ipv6->ttl;
-              g_rime_hdrlen         += SIXLOWPAN_HC1_HDR_LEN;
-            }
+                hc1[RIME_HC1_ENCODING] = 0xfa;
+                hc1[RIME_HC1_TTL]      = ipv6->ttl;
+                g_frame_hdrlen        += SIXLOWPAN_HC1_HDR_LEN;
+              }
+          }
           break;
 #endif /* CONFIG_NET_UDP */
         }
@@ -239,14 +247,16 @@ void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
  *   This function is called by the input function when the dispatch is
  *   HC1.  It processes the packet in the rime buffer, uncompresses the
  *   header fields, and copies the result in the sixlowpan buffer.  At the
- *   end of the decompression, g_rime_hdrlen and uncompressed_hdr_len
+ *   end of the decompression, g_frame_hdrlen and uncompressed_hdr_len
  *   are set to the appropriate values
  *
  * Input Parameters:
- *   ieee  - A reference to the IEE802.15.4 network device state
- *   iplen - Equal to 0 if the packet is not a fragment (IP length is then
- *           inferred from the L2 length), non 0 if the packet is a 1st
- *           fragment.
+ *   ieee   - A reference to the IEE802.15.4 network device state
+ *   iplen  - Equal to 0 if the packet is not a fragment (IP length is then
+ *            inferred from the L2 length), non 0 if the packet is a 1st
+ *            fragment.
+ *   iob    - Pointer to the IOB containing the received frame.
+ *   payptr - Pointer to the frame data payload.
  *
  * Returned Value:
  *   Zero (OK) is returned on success, on failure a negater errno value is
@@ -255,10 +265,11 @@ void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
  ****************************************************************************/
 
 int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
-                                uint16_t iplen)
+                                uint16_t iplen, FAR struct iob_s *iob,
+                                FAR uint8_t *payptr)
 {
   FAR struct ipv6_hdr_s *ipv6 = IPv6BUF(&ieee->i_dev);
-  FAR uint8_t *hc1 = RIME_HC1_PTR;
+  FAR uint8_t *hc1 = payptr + g_frame_hdrlen;
 
   /* Format the IPv6 header in the device d_buf */
   /* Set version, traffic clase, and flow label */
@@ -272,9 +283,9 @@ int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
    */
 
   sixlowpan_ipfromrime(&g_pktaddrs[PACKETBUF_ADDR_SENDER],
-                       &ipv6->srcipaddr);
+                       ipv6->srcipaddr);
   sixlowpan_ipfromrime(&g_pktaddrs[PACKETBUF_ADDR_RECEIVER],
-                       &ipv6->destipaddr);
+                       ipv6->destipaddr);
   g_uncomp_hdrlen += IPv6_HDRLEN;
 
   /* len[], proto, and ttl depend on the encoding */
@@ -282,16 +293,16 @@ int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
   switch (hc1[RIME_HC1_ENCODING] & 0x06)
     {
     case SIXLOWPAN_HC1_NH_ICMP6:
-      ipv6->proto    = IP_PROTO_ICMP6;
-      ipv6->ttl      = hc1[RIME_HC1_TTL];
-      g_rime_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
+      ipv6->proto     = IP_PROTO_ICMP6;
+      ipv6->ttl       = hc1[RIME_HC1_TTL];
+      g_frame_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
       break;
 
 #if CONFIG_NET_TCP
     case SIXLOWPAN_HC1_NH_TCP:
-      ipv6->proto    = IP_PROTO_TCP;
-      ipv6->ttl      = hc1[RIME_HC1_TTL];
-      g_rime_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
+      ipv6->proto     = IP_PROTO_TCP;
+      ipv6->ttl       = hc1[RIME_HC1_TTL];
+      g_frame_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
       break;
 #endif /* CONFIG_NET_TCP */
 
@@ -299,7 +310,7 @@ int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
     case SIXLOWPAN_HC1_NH_UDP:
       {
         FAR struct udp_hdr_s *udp = UDPIPv6BUF(&ieee->i_dev);
-        FAR uint8_t *hcudp = RIME_HC1_HC_UDP_PTR;
+        FAR uint8_t *hcudp = payptr + g_frame_hdrlen;
 
         ipv6->proto = IP_PROTO_UDP;
         if ((hcudp[RIME_HC1_HC_UDP_HC1_ENCODING] & 0x01) != 0)
@@ -320,18 +331,18 @@ int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
             /* UDP ports, len, checksum */
 
             udp->srcport =
-              htons(SIXLOWPAN_UDP_PORT_MIN + (hcudp[RIME_HC1_HC_UDP_PORTS] >> 4));
+              htons(CONFIG_NET_6LOWPAN_MINPORT + (hcudp[RIME_HC1_HC_UDP_PORTS] >> 4));
             udp->destport =
-              htons(SIXLOWPAN_UDP_PORT_MIN + (hcudp[RIME_HC1_HC_UDP_PORTS] & 0x0F));
+              htons(CONFIG_NET_6LOWPAN_MINPORT + (hcudp[RIME_HC1_HC_UDP_PORTS] & 0x0F));
 
             memcpy(&udp->udpchksum, &hcudp[RIME_HC1_HC_UDP_CHKSUM], 2);
 
-            g_uncomp_hdrlen += UIP_UDPH_LEN;
-            g_rime_hdrlen   += SIXLOWPAN_HC1_HC_UDP_HDR_LEN;
+            g_uncomp_hdrlen += UDP_HDRLEN;
+            g_frame_hdrlen  += SIXLOWPAN_HC1_HC_UDP_HDR_LEN;
           }
         else
           {
-            g_rime_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
+            g_frame_hdrlen  += SIXLOWPAN_HC1_HDR_LEN;
           }
       }
       break;
@@ -348,8 +359,8 @@ int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
       /* This is not a fragmented packet */
 
       ipv6->len[0] = 0;
-      ipv6->len[1] = ieee->i_dev.d_len - g_rime_hdrlen +  /* REVISIT */
-                     g_uncomp_hdrlen - IPv6_HDRLEN;
+      ipv6->len[1] = iob->io_len - g_frame_hdrlen + g_uncomp_hdrlen -
+                     IPv6_HDRLEN;
     }
   else
     {
@@ -359,9 +370,9 @@ int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
       ipv6->len[1] = (iplen - IPv6_HDRLEN) & 0x00FF;
     }
 
-  /* length field in UDP header */
-
 #if CONFIG_NET_UDP
+  /* Length field in UDP header */
+
   if (ipv6->proto == IP_PROTO_UDP)
     {
       FAR struct udp_hdr_s *udp = UDPIPv6BUF(&ieee->i_dev);
@@ -369,7 +380,7 @@ int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
     }
 #endif
 
-  return;
+  return OK;
 }
 
 #endif /* CONFIG_NET_6LOWPAN_COMPRESSION_HC1 */
diff --git a/net/sixlowpan/sixlowpan_input.c b/net/sixlowpan/sixlowpan_input.c
index 5ee8b49017f..ffc85bdfcfb 100644
--- a/net/sixlowpan/sixlowpan_input.c
+++ b/net/sixlowpan/sixlowpan_input.c
@@ -87,6 +87,94 @@
 
 #define IPv6BUF(dev)  ((FAR struct ipv6_hdr_s *)((dev)->d_buf))
 
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: sixlowpan_recv_hdrlen
+ *
+ * Description:
+ *   Get the length of the IEEE802.15.4 header on the received frame.
+ *
+ * Input Parameters:
+ *   ieee - The IEEE802.15.4 MAC network driver interface.
+ *   iob  - The IOB containing the frame.
+ *
+ * Returned Value:
+ *   Ok is returned on success; Othewise a negated errno value is returned.
+ *
+ * Assumptions:
+ *   Network is locked
+ *
+ ****************************************************************************/
+
+int sixlowpan_recv_hdrlen(FAR const uint8_t *fptr)
+{
+  uint16_t hdrlen;
+  uint8_t addrmode;
+
+  /* Minimum header:  2 byte FCF + 1 byte sequence number */
+
+  hdrlen = 3;
+
+  /* Account for destination address size */
+
+  addrmode = (fptr[1] & FRAME802154_DSTADDR_MASK) >> FRAME802154_DSTADDR_SHIFT;
+  if (addrmode == FRAME802154_SHORTADDRMODE)
+    {
+      /* 2 byte dest PAN + 2 byte dest short address */
+
+      hdrlen += 4;
+    }
+  else if (addrmode == FRAME802154_LONGADDRMODE)
+    {
+      /* 2 byte dest PAN + 6 byte dest long address */
+
+      hdrlen += 10;
+    }
+  else if (addrmode != FRAME802154_NOADDR)
+    {
+      nwarn("WARNING: Unrecognized address mode\n");
+
+      return -ENOSYS;
+    }
+  else if ((fptr[0] & (1 << FRAME802154_PANIDCOMP_SHIFT)) != 0)
+    {
+      nwarn("WARNING: PAN compression, but no destination address\n");
+
+      return -EINVAL;
+    }
+
+  /* Account for source address size */
+
+  addrmode = (fptr[1] & FRAME802154_SRCADDR_MASK) >> FRAME802154_SRCADDR_SHIFT;
+  if (addrmode == FRAME802154_NOADDR)
+    {
+      return hdrlen;
+    }
+  else
+    {
+      if ((fptr[0] & (1 << FRAME802154_PANIDCOMP_SHIFT)) == 0)
+        {
+          hdrlen += 2;
+        }
+
+      /* Add the length of the source address */
+
+      if (addrmode == FRAME802154_SHORTADDRMODE)
+        {
+          return hdrlen + 2;
+        }
+      else if (addrmode == FRAME802154_LONGADDRMODE)
+        {
+          return hdrlen + 8;
+        }
+    }
+
+  return 0;
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -121,12 +209,14 @@
 static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
                                    FAR struct iob_s *iob)
 {
-  FAR uint8_t *hc1 = RIME_HC1_PTR;
+  FAR uint8_t *payptr;        /* Pointer to the frame payload */
+  FAR uint8_t *hc1;           /* Convenience pointer to HC1 data */
 
   uint16_t fragsize  = 0;     /* Size of the IP packet (read from fragment) */
   uint8_t fragoffset = 0;     /* Offset of the fragment in the IP packet */
   bool isfrag        = false;
   int reqsize;                /* Required buffer size */
+  int hdrsize;                /* Size of the IEEE802.15.4 header */
 
 #if CONFIG_NET_6LOWPAN_FRAG
   bool isfirstfrag   = false;
@@ -140,28 +230,25 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
    */
 
   g_uncomp_hdrlen = 0;
-  g_rime_hdrlen = 0;
+  g_frame_hdrlen  = 0;
 
-  /* The MAC puts the 15.4 payload inside the RIME data buffer */
+  /* Get a pointer to the payload following the IEEE802.15.4 frame header. */
 
-  g_rimeptr = &iob->io_data[PACKETBUF_HDR_SIZE];
-
-#if CONFIG_NET_6LOWPAN_FRAG
-  /* If reassembly timed out, cancel it */
-
-  elapsed = clock_systimer() - ieee->i_time;
-  if (elapsed > NET_6LOWPAN_TIMEOUT)
+  hdrsize = sixlowpan_recv_hdrlen(iob->io_data);
+  if (hdrsize < 0)
     {
-      nwarn("WARNING: Reassembly timed out\n");
-      ieee->i_pktlen   = 0;
-      ieee->i_accumlen = 0;
+      nwarn("Invalid IEEE802.15.2 header: %d\n", hdrsize);
+      return hdrsize;
     }
 
+  payptr = &iob->io_data[hdrsize];
+
+#if CONFIG_NET_6LOWPAN_FRAG
   /* Since we don't support the mesh and broadcast header, the first header
    * we look for is the fragmentation header
    */
 
-  switch ((GETINT16(RIME_FRAG_PTR, RIME_FRAG_DISPATCH_SIZE) & 0xf800) >> 8)
+  switch ((GETINT16(payptr, RIME_FRAG_DISPATCH_SIZE) & 0xf800) >> 8)
     {
     /* First fragment of new reassembly */
 
@@ -170,13 +257,13 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
         /* Set up for the reassembly */
 
         fragoffset  = 0;
-        fragsize    = GETINT16(RIME_FRAG_PTR, RIME_FRAG_DISPATCH_SIZE) & 0x07ff;
-        fragtag     = GETINT16(RIME_FRAG_PTR, RIME_FRAG_TAG);
+        fragsize    = GETINT16(payptr, RIME_FRAG_DISPATCH_SIZE) & 0x07ff;
+        fragtag     = GETINT16(payptr, RIME_FRAG_TAG);
 
         ninfo("FRAG1: size %d, tag %d, offset %d\n",
               fragsize, fragtag, fragoffset);
 
-        g_rime_hdrlen += SIXLOWPAN_FRAG1_HDR_LEN;
+        g_frame_hdrlen += SIXLOWPAN_FRAG1_HDR_LEN;
 
         /* Indicate the first fragment of the reassembly */
 
@@ -189,17 +276,17 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
       {
         /* Set offset, tag, size.  Offset is in units of 8 bytes. */
 
-        fragoffset  = RIME_FRAG_PTR[RIME_FRAG_OFFSET];
-        fragtag     = GETINT16(RIME_FRAG_PTR, RIME_FRAG_TAG);
-        fragsize    = GETINT16(RIME_FRAG_PTR, RIME_FRAG_DISPATCH_SIZE) & 0x07ff;
+        fragoffset  = payptr[RIME_FRAG_OFFSET];
+        fragtag     = GETINT16(payptr, RIME_FRAG_TAG);
+        fragsize    = GETINT16(payptr, RIME_FRAG_DISPATCH_SIZE) & 0x07ff;
 
         ninfo("FRAGN: size %d, tag %d, offset %d\n",
               fragsize, fragtag, fragoffset);
 
-        g_rime_hdrlen += SIXLOWPAN_FRAGN_HDR_LEN;
+        g_frame_hdrlen += SIXLOWPAN_FRAGN_HDR_LEN;
 
         ninfo("islastfrag?: i_accumlen %d g_rime_payloadlen %d fragsize %d\n",
-              ieee->i_accumlen, iob->io_len - g_rime_hdrlen, fragsize);
+              ieee->i_accumlen, iob->io_len - g_frame_hdrlen, fragsize);
 
         /* Indicate that this frame is a another fragment for reassembly */
 
@@ -211,7 +298,7 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
          * bytes at the end. We must be liberal in what we accept.
          */
 
-        if (ieee->i_accumlen + iob->io_len - g_rime_hdrlen >= fragsize)
+        if (ieee->i_accumlen + iob->io_len - g_frame_hdrlen >= fragsize)
           {
             islastfrag = true;
           }
@@ -228,6 +315,16 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
 
   if (ieee->i_accumlen > 0)
     {
+      /* If reassembly timed out, cancel it */
+
+      elapsed = clock_systimer() - ieee->i_time;
+      if (elapsed > NET_6LOWPAN_TIMEOUT)
+        {
+          nwarn("WARNING: Reassembly timed out\n");
+          ieee->i_pktlen   = 0;
+          ieee->i_accumlen = 0;
+        }
+
       /* In this case what we expect is that the next frame will hold the
        * next FRAGN of the sequence.  We have to handle a few exeptional
        * cases that we need to handle:
@@ -245,7 +342,7 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
        *
        */
 
-      if (!isfrag)
+      else if (!isfrag)
         {
           /* Discard the partially assembled packet */
 
@@ -286,13 +383,15 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
                 "the packet currently being reassembled\n");
           return OK;
         }
+      else
+        {
+          /* Looks good.  We are currently processing a reassembling sequence
+           * and we recieved a valid FRAGN fragment.  Skip the header
+           * compression dispatch logic.
+           */
 
-      /* Looks good.  We are currently processing a reassembling sequence and
-       * we recieved a valid FRAGN fragment.  Skip the header compression
-       * dispatch logic.
-       */
-
-      goto copypayload;
+          goto copypayload;
+        }
     }
 
   /* There is no reassembly in progress. Check if we received a fragment */
@@ -339,11 +438,13 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
 
   /* Process next dispatch and headers */
 
+  hc1 = payptr + g_frame_hdrlen;
+
 #ifdef CONFIG_NET_6LOWPAN_COMPRESSION_HC06
   if ((hc1[RIME_HC1_DISPATCH] & SIXLOWPAN_DISPATCH_IPHC_MASK) == SIXLOWPAN_DISPATCH_IPHC)
     {
       ninfo("IPHC Dispatch\n");
-      sixlowpan_uncompresshdr_hc06(ieee, fragsize);
+      sixlowpan_uncompresshdr_hc06(ieee, fragsize, iob, payptr);
     }
   else
 #endif /* CONFIG_NET_6LOWPAN_COMPRESSION_HC06 */
@@ -352,22 +453,23 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
   if (hc1[RIME_HC1_DISPATCH] == SIXLOWPAN_DISPATCH_HC1)
     {
       ninfo("HC1 Dispatch\n");
-      sixlowpan_uncompresshdr_hc1(ieee, fragsize);
+      sixlowpan_uncompresshdr_hc1(ieee, fragsize, iob, payptr);
     }
   else
 #endif /* CONFIG_NET_6LOWPAN_COMPRESSION_HC1 */
+
   if (hc1[RIME_HC1_DISPATCH] == SIXLOWPAN_DISPATCH_IPV6)
     {
       ninfo("IPV6 Dispatch\n");
-      g_rime_hdrlen += SIXLOWPAN_IPV6_HDR_LEN;
+      g_frame_hdrlen  += SIXLOWPAN_IPV6_HDR_LEN;
 
       /* Put uncompressed IP header in d_buf. */
 
-      memcpy(ieee->i_dev.d_buf, g_rimeptr + g_rime_hdrlen, IPv6_HDRLEN);
+      memcpy(ieee->i_dev.d_buf, payptr + g_frame_hdrlen, IPv6_HDRLEN);
 
-      /* Update g_uncomp_hdrlen and g_rime_hdrlen. */
+      /* Update g_uncomp_hdrlen and g_frame_hdrlen. */
 
-      g_rime_hdrlen += IPv6_HDRLEN;
+      g_frame_hdrlen  += IPv6_HDRLEN;
       g_uncomp_hdrlen += IPv6_HDRLEN;
     }
   else
@@ -382,19 +484,20 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee,
 copypayload:
 #endif /* CONFIG_NET_6LOWPAN_FRAG */
 
-  /* Copy "payload" from the rime buffer to the d_buf.  If this frame is a
-   * first fragment or not part of a fragmented packet, we have already
-   * copied the compressed headers, g_uncomp_hdrlen and g_rime_hdrlen are
-   * non-zerio, fragoffset is.
+  /* Copy "payload" from the rime buffer to the IEEE802.15.4 MAC drivers
+   * d_buf.  If this frame is a first fragment or not part of a fragmented
+   * packet, we have already copied the compressed headers, g_uncomp_hdrlen
+   * and g_frame_hdrlen are non-zerio, fragoffset is.
    */
 
-  if (ieee->i_dev.d_len < g_rime_hdrlen)
+  if (g_frame_hdrlen > CONFIG_NET_6LOWPAN_MTU)
     {
-      ninfo("SIXLOWPAN: packet dropped due to header > total packet\n");
+      nwarn("WARNING: Packet dropped due to header (%u) > packet buffer (%u)\n",
+            g_frame_hdrlen, CONFIG_NET_6LOWPAN_MTU);
       return OK;
     }
 
-  g_rime_payloadlen = ieee->i_dev.d_len - g_rime_hdrlen;
+  g_rime_payloadlen = iob->io_len - g_frame_hdrlen;
 
   /* Sanity-check size of incoming packet to avoid buffer overflow */
 
@@ -408,7 +511,7 @@ copypayload:
     }
 
   memcpy((FAR uint8_t *)ieee->i_dev.d_buf + g_uncomp_hdrlen +
-         (int)(fragoffset << 3), g_rimeptr + g_rime_hdrlen,
+         (int)(fragoffset << 3), payptr + g_frame_hdrlen,
          g_rime_payloadlen);
 
 #if CONFIG_NET_6LOWPAN_FRAG
@@ -470,8 +573,9 @@ copypayload:
     }
 #endif /* CONFIG_NET_6LOWPAN_FRAG */
 
-  ninfodumpbuffer("IPv6 header", (FAR const uint8_t *)IPv6BUF(&ieee->i_dev),
-                  IPv6_HDRLEN);
+  sixlowpan_dumpbuffer("IPv6 header",
+                       (FAR const uint8_t *)IPv6BUF(&ieee->i_dev),
+                       IPv6_HDRLEN);
   return OK;
 }
 
@@ -491,6 +595,10 @@ copypayload:
 
 static int sixlowpan_dispatch(FAR struct ieee802154_driver_s *ieee)
 {
+  sixlowpan_dumpbuffer("Incoming packet",
+                       (FAR const uint8_t *)IPv6BUF(&ieee->i_dev),
+                       ieee->i_dev.d_len);
+
 #ifdef CONFIG_NET_PKT
   /* When packet sockets are enabled, feed the frame into the packet tap */
 
@@ -585,6 +693,9 @@ int sixlowpan_input(FAR struct ieee802154_driver_s *ieee)
       FRAME_IOB_REMOVE(ieee, iob);
       DEBUGASSERT(iob != NULL);
 
+      sixlowpan_dumpbuffer("Incoming frame",
+                           (FAR const uint8_t *)iob->io_data, iob->io_len);
+
       /* Process the frame, decompressing it into the packet buffer */
 
       ret = sixlowpan_frame_process(ieee, iob);
diff --git a/net/sixlowpan/sixlowpan_internal.h b/net/sixlowpan/sixlowpan_internal.h
index 4e8fc75d77e..1a4f28056e4 100644
--- a/net/sixlowpan/sixlowpan_internal.h
+++ b/net/sixlowpan/sixlowpan_internal.h
@@ -100,21 +100,16 @@
  * bytes for all subsequent headers.
  */
 
-#define RIME_FRAG_PTR                   g_rimeptr
 #define RIME_FRAG_DISPATCH_SIZE         0  /* 16 bit */
 #define RIME_FRAG_TAG                   2  /* 16 bit */
 #define RIME_FRAG_OFFSET                4  /* 8 bit */
 
 /* Define the Rime buffer as a byte array */
 
-#define RIME_IPHC_BUF                   (g_rimeptr + g_rime_hdrlen)
-
-#define RIME_HC1_PTR                    (g_rimeptr + g_rime_hdrlen)
 #define RIME_HC1_DISPATCH               0  /* 8 bit */
 #define RIME_HC1_ENCODING               1  /* 8 bit */
 #define RIME_HC1_TTL                    2  /* 8 bit */
 
-#define RIME_HC1_HC_UDP_PTR             (g_rimeptr + g_rime_hdrlen)
 #define RIME_HC1_HC_UDP_DISPATCH        0  /* 8 bit */
 #define RIME_HC1_HC_UDP_HC1_ENCODING    1  /* 8 bit */
 #define RIME_HC1_HC_UDP_UDP_ENCODING    2  /* 8 bit */
@@ -126,33 +121,47 @@
  * IEEE802.15.4 spec for details.
  */
 
-#define FRAME802154_BEACONFRAME         0x00
-#define FRAME802154_DATAFRAME           0x01
-#define FRAME802154_ACKFRAME            0x02
-#define FRAME802154_CMDFRAME            0x03
+#define FRAME802154_FRAMETYPE_SHIFT     (0)  /* Bits 0-2: Frame type */
+#define FRAME802154_FRAMETYPE_MASK      (7 << FRAME802154_FRAMETYPE_SHIFT)
+#define FRAME802154_SECENABLED_SHIFT    (3)  /* Bit 3: Security enabled */
+#define FRAME802154_FRAMEPENDING_SHIFT  (4)  /* Bit 4: Frame pending */
+#define FRAME802154_ACKREQUEST_SHIFT    (5)  /* Bit 5: ACK request */
+#define FRAME802154_PANIDCOMP_SHIFT     (6)  /* Bit 6: PANID compression */
+                                             /* Bits 7-9: Reserved */
+#define FRAME802154_DSTADDR_SHIFT       (2)  /* Bits 10-11: Dest address mode */
+#define FRAME802154_DSTADDR_MASK        (3 << FRAME802154_DSTADDR_SHIFT)
+#define FRAME802154_VERSION_SHIFT       (4)  /* Bit 12-13: Frame version */
+#define FRAME802154_VERSION_MASK        (3 << FRAME802154_VERSION_SHIFT)
+#define FRAME802154_SRCADDR_SHIFT       (6)  /* Bits 14-15: Source address mode */
+#define FRAME802154_SRCADDR_MASK        (3 << FRAME802154_SRCADDR_SHIFT)
 
-#define FRAME802154_BEACONREQ           0x07
+/* Unshifted values for use in struct frame802154_fcf_s */
 
-#define FRAME802154_IEEERESERVED        0x00
-#define FRAME802154_NOADDR              0x00  /* Only valid for ACK or Beacon frames */
-#define FRAME802154_SHORTADDRMODE       0x02
-#define FRAME802154_LONGADDRMODE        0x03
+#define FRAME802154_BEACONFRAME         (0)
+#define FRAME802154_DATAFRAME           (1)
+#define FRAME802154_ACKFRAME            (2)
+#define FRAME802154_CMDFRAME            (3)
+
+#define FRAME802154_BEACONREQ           (7)
+
+#define FRAME802154_IEEERESERVED        (0)
+#define FRAME802154_NOADDR              (0)  /* Only valid for ACK or Beacon frames */
+#define FRAME802154_SHORTADDRMODE       (2)
+#define FRAME802154_LONGADDRMODE        (3)
 
 #define FRAME802154_NOBEACONS           0x0f
 
 #define FRAME802154_BROADCASTADDR       0xffff
 #define FRAME802154_BROADCASTPANDID     0xffff
 
-#define FRAME802154_IEEE802154_2003     0x00
-#define FRAME802154_IEEE802154_2006     0x01
+#define FRAME802154_IEEE802154_2003     (0)
+#define FRAME802154_IEEE802154_2006     (1)
 
-#define FRAME802154_SECURITY_LEVEL_NONE 0
-#define FRAME802154_SECURITY_LEVEL_128  3
+#define FRAME802154_SECURITY_LEVEL_NONE (0)
+#define FRAME802154_SECURITY_LEVEL_128  (3)
 
 /* Packet buffer Definitions */
 
-#define PACKETBUF_HDR_SIZE                    48
-
 #define PACKETBUF_ATTR_PACKET_TYPE_DATA       0
 #define PACKETBUF_ATTR_PACKET_TYPE_ACK        1
 #define PACKETBUF_ATTR_PACKET_TYPE_STREAM     2
@@ -300,6 +309,14 @@
     } \
   while(0)
 
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_NET_6LOWPAN_DUMPBUFFER
+#  define sixlowpan_dumpbuffer(m,b,s) ninfodumpbuffer(m,b,s)
+#else
+#  define sixlowpan_dumpbuffer(m,b,s)
+#endif
+
 /****************************************************************************
  * Public Types
  ****************************************************************************/
@@ -408,7 +425,7 @@ struct frame802154_s
 /* A pointer to the rime buffer.
  *
  * We initialize it to the beginning of the rime buffer, then access
- * different fields by updating the offset ieee->g_rime_hdrlen.
+ * different fields by updating the offset ieee->g_frame_hdrlen.
  */
 
 extern FAR uint8_t *g_rimeptr;
@@ -428,12 +445,12 @@ extern uint8_t g_rime_payloadlen;
 
 extern uint8_t g_uncomp_hdrlen;
 
-/* g_rime_hdrlen is the total length of (the processed) 6lowpan headers
+/* g_frame_hdrlen is the total length of (the processed) 6lowpan headers
  * (fragment headers, IPV6 or HC1, HC2, and HC1 and HC2 non compressed
  * fields).
  */
 
-extern uint8_t g_rime_hdrlen;
+extern uint8_t g_frame_hdrlen;
 
 /* Offset first available byte for the payload after header region. */
 
@@ -497,7 +514,7 @@ int sixlowpan_send(FAR struct net_driver_s *dev,
                    uint16_t timeout);
 
 /****************************************************************************
- * Function: sixlowpan_hdrlen
+ * Function: sixlowpan_send_hdrlen
  *
  * Description:
  *   This function is before the first frame has been sent in order to
@@ -515,7 +532,7 @@ int sixlowpan_send(FAR struct net_driver_s *dev,
  *
  ****************************************************************************/
 
-int sixlowpan_hdrlen(FAR struct ieee802154_driver_s *ieee,
+int sixlowpan_send_hdrlen(FAR struct ieee802154_driver_s *ieee,
                      uint16_t dest_panid);
 
 /****************************************************************************
@@ -548,7 +565,7 @@ int sixlowpan_framecreate(FAR struct ieee802154_driver_s *ieee,
  *   send interrupt logic when a TX poll is received.  It formates the
  *   list of frames to be sent by the IEEE802.15.4 MAC driver.
  *
- *   The payload data is in the caller 's_buf' and is of length 's_len'.
+ *   The payload data is in the caller 'buf' and is of length 'buflen'.
  *   Compressed headers will be added and if necessary the packet is
  *   fragmented. The resulting packet/fragments are put in ieee->i_framelist
  *   and the entire list of frames will be delivered to the 802.15.4 MAC via
@@ -557,8 +574,9 @@ int sixlowpan_framecreate(FAR struct ieee802154_driver_s *ieee,
  * Input Parameters:
  *   ieee    - The IEEE802.15.4 MAC driver instance
  *   ipv6hdr - IPv6 header followed by TCP or UDP header.
- *   buf     - Data to send
- *   buflen  - Length of data to send
+ *   buf     - Beginning of the packet packet to send (with IPv6 + protocol
+ *             headers)
+ *   buflen  - Length of data to send (include IPv6 and protocol headers)
  *   destmac - The IEEE802.15.4 MAC address of the destination
  *
  * Returned Value:
@@ -621,7 +639,7 @@ void sixlowpan_hc06_initialize(void);
  *   ipv6     - The IPv6 header to be compressed
  *   destmac  - L2 destination address, needed to compress the IP
  *              destination field
- *   iob      - The IOB into which the compressed header should be saved.
+ *   fptr     - Pointer to frame to be compressed.
  *
  * Returned Value:
  *   None
@@ -632,7 +650,7 @@ void sixlowpan_hc06_initialize(void);
 void sixlowpan_compresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
                                 FAR const struct ipv6_hdr_s *ipv6,
                                 FAR const struct rimeaddr_s *destmac,
-                                FAR struct iob_s *iob);
+                                FAR uint8_t *fptr);
 #endif
 
 /****************************************************************************
@@ -645,14 +663,16 @@ void sixlowpan_compresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
  *   This function is called by the input function when the dispatch is HC06.
  *   We process the packet in the rime buffer, uncompress the header fields,
  *   and copy the result in the sixlowpan buffer.  At the end of the
- *   decompression, g_rime_hdrlen and g_uncompressed_hdrlen are set to the
+ *   decompression, g_frame_hdrlen and g_uncompressed_hdrlen are set to the
  *   appropriate values
  *
  * Input Parmeters:
- *   ieee  - A reference to the IEE802.15.4 network device state
- *   iplen - Equal to 0 if the packet is not a fragment (IP length is then
- *           inferred from the L2 length), non 0 if the packet is a first
- *           fragment.
+ *   ieee   - A reference to the IEE802.15.4 network device state
+ *   iplen  - Equal to 0 if the packet is not a fragment (IP length is then
+ *            inferred from the L2 length), non 0 if the packet is a first
+ *            fragment.
+ *   iob    - Pointer to the IOB containing the received frame.
+ *   payptr - Pointer to the frame data payload.
  *
  * Returned Value:
  *   None
@@ -661,7 +681,8 @@ void sixlowpan_compresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
 
 #ifdef CONFIG_NET_6LOWPAN_COMPRESSION_HC06
 void sixlowpan_uncompresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
-                                  uint16_t iplen);
+                                  uint16_t iplen, FAR struct iob_s *iob,
+                                  FAR uint8_t *payptr);
 #endif
 
 /****************************************************************************
@@ -679,7 +700,7 @@ void sixlowpan_uncompresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
  *   ipv6    - The IPv6 header to be compressed
  *   destmac - L2 destination address, needed to compress the IP
  *             destination field
- *   iob     - The IOB into which the compressed header should be saved.
+ *   fptr     - Pointer to frame to be compressed.
  *
  * Returned Value:
  *   None
@@ -690,7 +711,7 @@ void sixlowpan_uncompresshdr_hc06(FAR struct ieee802154_driver_s *ieee,
 void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
                                FAR const struct ipv6_hdr_s *ipv6,
                                FAR const struct rimeaddr_s *destmac,
-                               FAR struct iob_s *iob);
+                               FAR uint8_t *fptr);
 #endif
 
 /****************************************************************************
@@ -702,7 +723,7 @@ void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
  *   This function is called by the input function when the dispatch is
  *   HC1.  It processes the packet in the rime buffer, uncompresses the
  *   header fields, and copies the result in the sixlowpan buffer.  At the
- *   end of the decompression, g_rime_hdrlen and uncompressed_hdr_len
+ *   end of the decompression, g_frame_hdrlen and uncompressed_hdr_len
  *   are set to the appropriate values
  *
  * Input Parameters:
@@ -710,6 +731,8 @@ void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
  *   iplen - Equal to 0 if the packet is not a fragment (IP length is then
  *           inferred from the L2 length), non 0 if the packet is a first
  *           fragment.
+ *   iob    - Pointer to the IOB containing the received frame.
+ *   payptr - Pointer to the frame data payload.
  *
  * Returned Value:
  *   None
@@ -717,8 +740,9 @@ void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
  ****************************************************************************/
 
 #ifdef CONFIG_NET_6LOWPAN_COMPRESSION_HC1
-void sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
-                                 uint16_t ip_len);
+int sixlowpan_uncompresshdr_hc1(FAR struct ieee802154_driver_s *ieee,
+                                uint16_t ip_len, FAR struct iob_s *iob,
+                                FAR uint8_t *payptr);
 #endif
 
 /****************************************************************************
@@ -752,7 +776,7 @@ int sixlowpan_frame_hdralloc(FAR struct iob_s *iob, int size);
  *
  ****************************************************************************/
 
-#define sixlowpan_islinklocal(ipaddr) ((ipaddr)[0] = NTOHS(0xfe80))
+#define sixlowpan_islinklocal(ipaddr) ((ipaddr)[0] == NTOHS(0xfe80))
 
 void sixlowpan_ipfromrime(FAR const struct rimeaddr_s *rime,
                           net_ipv6addr_t ipaddr);
diff --git a/net/sixlowpan/sixlowpan_tcpsend.c b/net/sixlowpan/sixlowpan_tcpsend.c
index 7beceee5c55..1ad8068a317 100644
--- a/net/sixlowpan/sixlowpan_tcpsend.c
+++ b/net/sixlowpan/sixlowpan_tcpsend.c
@@ -55,6 +55,14 @@
 
 #if defined(CONFIG_NET_6LOWPAN) && defined(CONFIG_NET_TCP)
 
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Buffer access helpers */
+
+#define IPv6BUF(dev)  ((FAR struct ipv6_hdr_s *)((dev)->d_buf))
+
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
@@ -156,6 +164,7 @@ ssize_t psock_6lowpan_tcp_send(FAR struct socket *psock, FAR const void *buf,
   int ret;
 
   ninfo("buflen %lu\n", (unsigned long)buflen);
+  sixlowpan_dumpbuffer("Outgoing TCP payload", buf, buflen);
 
   DEBUGASSERT(psock != NULL && psock->s_crefs > 0);
   DEBUGASSERT(psock->s_type == SOCK_STREAM);
@@ -368,6 +377,8 @@ void sixlowpan_tcp_send(FAR struct net_driver_s *dev)
   /* Double check */
 
   ninfo("d_len %u\n", dev->d_len);
+  sixlowpan_dumpbuffer("Outgoing TCP packet",
+                       (FAR const uint8_t *)IPv6BUF(dev), dev->d_len);
 
   if (dev != NULL && dev->d_len > 0)
     {
@@ -390,30 +401,19 @@ void sixlowpan_tcp_send(FAR struct net_driver_s *dev)
         }
       else
         {
-          size_t hdrlen;
+          struct rimeaddr_s destmac;
 
-          hdrlen = IPv6_HDRLEN + TCP_HDRLEN;
-          if (hdrlen > dev->d_len)
-            {
-              nwarn("WARNING: Packet to small:  Have %u need >%u\n",
-                    dev->d_len, hdrlen);
-            }
-          else
-            {
-              struct rimeaddr_s destmac;
+          /* Get the Rime MAC address of the destination.  This assumes an
+           * encoding of the MAC address in the IPv6 address.
+           */
 
-              /* Get the Rime MAC address of the destination.  This assumes
-               * an encoding of the MAC address in the IPv6 address.
-               */
+          sixlowpan_rimefromip(ipv6hdr->destipaddr, &destmac);
 
-              sixlowpan_rimefromip(ipv6hdr->destipaddr, &destmac);
+          /* Convert the outgoing packet into a frame list. */
 
-              /* Convert the outgoing packet into a frame list. */
-
-              (void)sixlowpan_queue_frames(
-                      (FAR struct ieee802154_driver_s *)dev, ipv6hdr,
-                      dev->d_buf + hdrlen, dev->d_len - hdrlen, &destmac);
-            }
+          (void)sixlowpan_queue_frames(
+                  (FAR struct ieee802154_driver_s *)dev, ipv6hdr,
+                  dev->d_buf, dev->d_len, &destmac);
         }
     }
 
diff --git a/net/sixlowpan/sixlowpan_udpsend.c b/net/sixlowpan/sixlowpan_udpsend.c
index 53c7ead6ce8..717df424a27 100644
--- a/net/sixlowpan/sixlowpan_udpsend.c
+++ b/net/sixlowpan/sixlowpan_udpsend.c
@@ -167,9 +167,13 @@ ssize_t psock_6lowpan_udp_sendto(FAR struct socket *psock,
   uint16_t timeout;
   int ret;
 
+  ninfo("buflen %lu\n", (unsigned long)buflen);
+
   DEBUGASSERT(psock != NULL && psock->s_crefs > 0 && to != NULL);
   DEBUGASSERT(psock->s_type == SOCK_DGRAM);
 
+  sixlowpan_dumpbuffer("Outgoing UDP payload", buf, buflen);
+
   if (psock == NULL || to == NULL)
     {
       return (ssize_t)-EINVAL;
@@ -352,9 +356,13 @@ ssize_t psock_6lowpan_udp_send(FAR struct socket *psock, FAR const void *buf,
   FAR struct udp_conn_s *conn;
   struct sockaddr_in6 to;
 
+  ninfo("buflen %lu\n", (unsigned long)buflen);
+
   DEBUGASSERT(psock != NULL && psock->s_crefs > 0);
   DEBUGASSERT(psock->s_type == SOCK_DGRAM);
 
+  sixlowpan_dumpbuffer("Outgoing UDP payload", buf, buflen);
+
   /* Make sure that this is a valid socket */
 
   if (psock != NULL || psock->s_crefs <= 0)
diff --git a/net/sixlowpan/sixlowpan_utils.c b/net/sixlowpan/sixlowpan_utils.c
index 7dd4e9e35c9..1ec8eec2b6c 100644
--- a/net/sixlowpan/sixlowpan_utils.c
+++ b/net/sixlowpan/sixlowpan_utils.c
@@ -149,7 +149,7 @@ void sixlowpan_rimefromip(const net_ipv6addr_t ipaddr,
 {
   /* REVISIT: See notes about 2 byte addresses in sixlowpan_ipfromrime() */
 
-  DEBUGASSERT(ipaddr[0] == 0xfe80);
+  DEBUGASSERT(ipaddr[0] == HTONS(0xfe80));
 
   memcpy(rime, &ipaddr[4], CONFIG_NET_6LOWPAN_RIMEADDR_SIZE);
   rime->u8[0] ^= 0x02;