CRC16 correctur by get Info (0x85)

[owSlave2.git] / common / OWRomFunctions.s

diff --git a/common/OWRomFunctions.s b/common/OWRomFunctions.s
index eac94b9..30731e7 100644 (file)
--- a/common/OWRomFunctions.s
+++ b/common/OWRomFunctions.s
@@ -320,8 +320,6 @@ h_fwconfiginfo:
        cpi  r_bytep,17\r
        breq h_fwconfiginfo_all\r
 #elif defined _CRC16_\r
-       cpi  r_bytep,17\r
-       breq h_fwconfiginfo_crc2\r
        cpi  r_bytep,18\r
        breq h_fwconfiginfo_all\r
 #else\r
@@ -329,15 +327,23 @@ h_fwconfiginfo:
        breq h_fwconfiginfo_all\r
 #warning No CRC known code implemented\r
 #endif\r
+h_fwconfiginfo_end:\r
        configZ config_info,r_bytep\r
        ld   r_rwbyte,Z\r
        rjmp handle_end_inc\r
 h_fwconfiginfo_crc:\r
+#ifdef _CRC8_\r
        lds r_rwbyte,crc\r
        rjmp handle_end_inc\r
-h_fwconfiginfo_crc2:\r
-       lds r_rwbyte,crc+1\r
-       rjmp handle_end_inc\r
+#elif defined _CRC16_\r
+       lds r_temp,crc\r
+       com r_temp\r
+       sts config_info+16,r_temp\r
+       lds r_temp,crc+1\r
+       com r_temp\r
+       sts config_info+17,r_temp\r
+       rjmp h_fwconfiginfo_end\r
+#endif\r
 h_fwconfiginfo_all:\r
        rjmp handle_end_sleep\r
 \r