Diff: HomemadeMbed.cpp

Revision:

15:b38d9d210e32

Parent:

14:2f89279586cb

Child:

16:c77e34bc69bc

--- a/HomemadeMbed.cpp	Mon Jan 16 02:28:40 2017 +0000
+++ b/HomemadeMbed.cpp	Tue Jan 17 00:06:38 2017 +0000
@@ -1,6 +1,7 @@
 #include "HomemadeMbed.hpp"
 #include "Utility.hpp"
-#include <cstdio>
+#include <cassert>
+#include "mbed.h"
 
 namespace homemade_mbed
 {
@@ -14,10 +15,36 @@
     
     void write_bits(unsigned int* address_4_bytes, const int start_bit, const int stop_bit, const unsigned int new_value)
     {
-        //std::printf("write_bits()\r\n");
-
         const unsigned int all_4_bytes = *address_4_bytes;
-        const unsigned int new_bits = utility::update_bits(all_4_bytes, start_bit, stop_bit, new_value);
+        const unsigned int reserved_bits_mask = get_reserved_bits_mask(address_4_bytes);
+        const unsigned int new_bits = utility::update_bits(all_4_bytes, start_bit, stop_bit, reserved_bits_mask, new_value);
         *address_4_bytes = new_bits;
+        
+        assert(new_value == homemade_mbed::read_bits(address_4_bytes, start_bit, stop_bit));
+    }
+    
+    unsigned int get_reserved_bits_mask(unsigned int* address_4_bytes)
+    {
+        switch((unsigned int)address_4_bytes)
+        {
+            case 0x400FC1C8: return 0x000003FF;
+            case 0x400FC0C4: return 0xEFEFF7DE;
+            case 0x4000C000: return 0x000000FF;
+            case 0x40098000: return 0x000000FF;
+            case 0x4009C000: return 0x000000FF;
+            case 0x4009800C: return 0x000000FF;
+            case 0x4009C00C: return 0x000000FF;
+            case 0x4000C00C: return 0x000000FF;
+            case 0x400FC088: return 0x07FF7FFF;
+            case 0x4000C008: return 0x000003CF;
+            case 0x40098008: return 0x000003CF;
+            case 0x4007C008: return 0x000000CF;
+            default: break; //Unknown address. Should never happen
+        }
+        
+        //Crash
+        printf("Unknown address: %d\r\n", (unsigned int)address_4_bytes);
+        assert(false); 
+        return 0xFFFFFFFF;
     }
 }
\ No newline at end of file