diff --git a/bacnet-stack/ports/atmega168/Makefile b/bacnet-stack/ports/atmega168/Makefile
index 8a485445..81189598 100644
--- a/bacnet-stack/ports/atmega168/Makefile
+++ b/bacnet-stack/ports/atmega168/Makefile
@@ -36,6 +36,7 @@ BACNET_DEMO = ../../demo
 # local files for this project
 CSRC = main.c \
 	timer.c \
+	stack.c \
 	rs485.c \
 	dlmstp.c \
 	apdu.c \
diff --git a/bacnet-stack/ports/atmega168/bacnet.ewp b/bacnet-stack/ports/atmega168/bacnet.ewp
index 7c8d20a3..bf0d3369 100644
--- a/bacnet-stack/ports/atmega168/bacnet.ewp
+++ b/bacnet-stack/ports/atmega168/bacnet.ewp
@@ -2032,6 +2032,9 @@
   <file>
     <name>$PROJ_DIR$\timer.c</name>
   </file>
+  <file>
+    <name>$PROJ_DIR$\stack.c</name>
+  </file>
   <file>
     <name>$PROJ_DIR$\timer.h</name>
   </file>
diff --git a/bacnet-stack/ports/atmega168/device.c b/bacnet-stack/ports/atmega168/device.c
index 62dc0dfb..c291e3f7 100644
--- a/bacnet-stack/ports/atmega168/device.c
+++ b/bacnet-stack/ports/atmega168/device.c
@@ -34,6 +34,7 @@
 #include "dlmstp.h"
 #include "rs485.h"
 #include "version.h"
+#include "stack.h"
 /* objects */
 #include "device.h"
 #include "av.h"
@@ -160,11 +161,6 @@ bool Device_Object_List_Identifier(
     return status;
 }
 
-#if defined(__GNUC__)
-extern uint8_t _end;
-extern uint8_t __stack;
-#endif
-
 /* return the length of the apdu encoded or -1 for error */
 int Device_Encode_Property_APDU(
     uint8_t * apdu,
@@ -326,43 +322,12 @@ int Device_Encode_Property_APDU(
             apdu_len =
                 encode_application_unsigned(&apdu[0], RS485_Get_Baud_Rate());
             break;
-#if defined(__GNUC__)
         case 512:
-            count = 1 + (&__stack) - (&_end);
-            if (array_index == 0) {
-                /* Array element zero is the number of bytes of stack */
-                apdu_len = encode_application_unsigned(&apdu[0], count);
-            } else if (array_index == BACNET_ARRAY_ALL) {
-                /* if no index was specified, then try to encode the entire list */
-                /* into one packet.  Note that more than likely you will have */
-                /* to return an error if the number of encoded objects exceeds */
-                /* your maximum APDU size. */
-                for (i = 1; i <= count; i++) {
-                    len = encode_application_unsigned(&apdu[0], *(&_end + i));
-                    apdu_len += len;
-                    /* assume next one is the same size as this one */
-                    /* can we all fit into the APDU? */
-                    if ((apdu_len + len) >= MAX_APDU) {
-                        *error_class = ERROR_CLASS_SERVICES;
-                        *error_code = ERROR_CODE_NO_SPACE_FOR_OBJECT;
-                        apdu_len = -1;
-                        break;
-                    }
-                }
-            } else if (array_index <= count) {
-                apdu_len =
-                    encode_application_unsigned(&apdu[0],
-                    *(&_end + array_index));
-            } else {
-                *error_class = ERROR_CLASS_PROPERTY;
-                *error_code = ERROR_CODE_INVALID_ARRAY_INDEX;
-                apdu_len = -1;
-            }
+            apdu_len = encode_application_unsigned(&apdu[0], stack_size());
             break;
         case 513:
-            apdu_len = encode_application_unsigned(&apdu[0], __stack);
+            apdu_len = encode_application_unsigned(&apdu[0], stack_unused());
             break;
-#endif
         default:
             *error_class = ERROR_CLASS_PROPERTY;
             *error_code = ERROR_CODE_UNKNOWN_PROPERTY;
diff --git a/bacnet-stack/ports/atmega168/main.c b/bacnet-stack/ports/atmega168/main.c
index b2010855..8672f4e2 100644
--- a/bacnet-stack/ports/atmega168/main.c
+++ b/bacnet-stack/ports/atmega168/main.c
@@ -139,34 +139,6 @@ static void input_switch_read(
     }
 }
 
-/* stack checking */
-#if defined(__GNUC__)
-extern uint8_t _end;
-extern uint8_t __stack;
-#define STACK_CANARY (0xC5)
-void StackPaint(
-    void) __attribute__ ((naked)) __attribute__ ((section(".init1")));
-
-void StackPaint(
-    void)
-{
-#if 0
-    uint8_t *p = &_end;
-
-    while (p <= &__stack) {
-        *p = STACK_CANARY;
-        p++;
-    }
-#else
-    __asm volatile (
-        "    ldi r30,lo8(_end)\n" "    ldi r31,hi8(_end)\n" "    ldi r24,lo8(0xc5)\n"   /* STACK_CANARY = 0xc5 */
-        "    ldi r25,hi8(__stack)\n" "    rjmp .cmp\n" ".loop:\n"
-        "    st Z+,r24\n" ".cmp:\n" "    cpi r30,lo8(__stack)\n"
-        "    cpc r31,r25\n" "    brlo .loop\n" "    breq .loop"::);
-#endif
-}
-#endif
-
 static uint8_t PDUBuffer[MAX_MPDU];
 int main(
     void)
diff --git a/bacnet-stack/ports/atmega168/stack.c b/bacnet-stack/ports/atmega168/stack.c
new file mode 100644
index 00000000..217e3452
--- /dev/null
+++ b/bacnet-stack/ports/atmega168/stack.c
@@ -0,0 +1,94 @@
+/**************************************************************************
+*
+* Copyright (C) 2009 Steve Karg <skarg@users.sourceforge.net>
+*
+* Permission is hereby granted, free of charge, to any person obtaining
+* a copy of this software and associated documentation files (the
+* "Software"), to deal in the Software without restriction, including
+* without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to
+* permit persons to whom the Software is furnished to do so, subject to
+* the following conditions:
+*
+* The above copyright notice and this permission notice shall be included
+* in all copies or substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*
+*********************************************************************/
+#include "hardware.h"
+
+/* stack checking */
+#if defined(__GNUC__)
+extern uint8_t _end;
+extern uint8_t __stack;
+#endif 
+
+#if defined(__GNUC__)
+#define STACK_CANARY (0xC5)
+void stack_init(
+    void) __attribute__ ((naked)) __attribute__ ((section(".init1")));
+#endif
+
+void stack_init(
+    void)
+{
+#if defined(__GNUC__)
+#if 0
+    uint8_t *p = &_end;
+
+    while (p <= &__stack) {
+        *p = STACK_CANARY;
+        p++;
+    }
+#else
+    __asm volatile (
+        "    ldi r30,lo8(_end)\n" "    ldi r31,hi8(_end)\n" "    ldi r24,lo8(0xc5)\n"   /* STACK_CANARY = 0xc5 */
+        "    ldi r25,hi8(__stack)\n" "    rjmp .cmp\n" ".loop:\n"
+        "    st Z+,r24\n" ".cmp:\n" "    cpi r30,lo8(__stack)\n"
+        "    cpc r31,r25\n" "    brlo .loop\n" "    breq .loop"::);
+#endif
+#endif
+}
+
+unsigned stack_size(void)
+{
+#if defined(__GNUC__)
+    return (&__stack) - (&_end);
+#else
+    return 0;
+#endif
+}
+
+uint8_t stack_byte(unsigned offset)
+{
+#if defined(__GNUC__)
+    return *(&_end + offset);
+#else
+    offset = offset;
+    return 0;
+#endif
+}
+
+unsigned stack_unused(void)
+{
+    unsigned count = 0;
+#if defined(__GNUC__)
+    uint8_t *p = &_end;
+
+    while (p <= &__stack) {
+        if ((*p) != STACK_CANARY) {
+            count = p - (&_end);
+            break;
+        }
+        p++;
+    }
+#endif
+    return count;
+}
diff --git a/bacnet-stack/ports/atmega168/stack.h b/bacnet-stack/ports/atmega168/stack.h
new file mode 100644
index 00000000..cf97bb7f
--- /dev/null
+++ b/bacnet-stack/ports/atmega168/stack.h
@@ -0,0 +1,48 @@
+/**************************************************************************
+*
+* Copyright (C) 2009 Steve Karg <skarg@users.sourceforge.net>
+*
+* Permission is hereby granted, free of charge, to any person obtaining
+* a copy of this software and associated documentation files (the
+* "Software"), to deal in the Software without restriction, including
+* without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to
+* permit persons to whom the Software is furnished to do so, subject to
+* the following conditions:
+*
+* The above copyright notice and this permission notice shall be included
+* in all copies or substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*
+*********************************************************************/
+#ifndef STACK_H
+#define STACK_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+    /* C stack checking */
+    void stack_init(void);
+
+    unsigned stack_size(void);
+
+    uint8_t stack_byte(unsigned offset);
+
+    unsigned stack_unused(void);
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+#endif
+    
\ No newline at end of file