diff --git a/App/main.c b/App/main.c
index 6ce26ae..ec76e8b 100644
--- a/App/main.c
+++ b/App/main.c
@@ -149,7 +149,6 @@ void WS2812_EncodeByte5(u8 v, u8 *buf)
 void WS2812_Write24Bit(u8 g, u8 r, u8 b)
 {
     u8 xdata buf[15];
-    u8 m1_save, m0_save;
     u8 i;
     
     WS2812_EncodeByte5(g, &buf[0]);
@@ -158,34 +157,23 @@ void WS2812_Write24Bit(u8 g, u8 r, u8 b)
     
     EA = 0;
     
-    m1_save = P2M1 & (1 << 5);
-    m0_save = P2M0 & (1 << 5);
-    
-    P2M1 |= (1 << 5);
-    P2M0 &= ~(1 << 5);
+    // Ensure P2.3 (MOSI) and P2.5 (SCLK) are configured as push-pull output
+    P2M1 &= ~((1 << 3) | (1 << 5));
+    P2M0 |=  ((1 << 3) | (1 << 5));
     
     SPCTL = 0xD0; // Enable SPI at SYSCLK/4 (6.0 MHz)
     SPSTAT = 0xC0; // Clear flags
     
-    SPDAT = buf[0];
-    SPDAT = buf[1];
-    
-    for (i = 2; i < 15; i++)
+    for (i = 0; i < 15; i++)
     {
+        SPDAT = buf[i];
         while (!(SPSTAT & 0x80));
         SPSTAT = 0xC0;
-        SPDAT = buf[i];
     }
-    while (!(SPSTAT & 0x80));
-    SPSTAT = 0xC0;
-    while (!(SPSTAT & 0x80));
-    SPSTAT = 0xC0;
     
     SPCTL = 0x90; // Disable SPI
     WS2812_DI = 0;
-    
-    P2M1 = (P2M1 & ~(1 << 5)) | m1_save;
-    P2M0 = (P2M0 & ~(1 << 5)) | m0_save;
+    MOTOR = 0;   // Ensure motor is off
     
     EA = 1;
 }