Fix function layer actions

[qmk_firmware.git] / quantum / light_ws2812.c

diff --git a/quantum/light_ws2812.c b/quantum/light_ws2812.c
index d38dac4c69f85f521711502db74b2b7872e7ffb3..55bdd9cd81929dffea09310730243c91d49f77c2 100755 (executable)
--- a/quantum/light_ws2812.c
+++ b/quantum/light_ws2812.c
@@ -16,8 +16,6 @@
 #include <util/delay.h>
 #include "debug.h"
 
-#define RGBW_BB_TWI 1
-
 #ifdef RGBW_BB_TWI
 
 // Port for the I2C
@@ -72,7 +70,7 @@ void I2C_WriteBit(unsigned char c)
 
 // Inits bitbanging port, must be called before using the functions below
 //
-void I2C_Init()
+void I2C_Init(void)
 {
     I2C_PORT &= ~ ((1 << I2C_DAT) | (1 << I2C_CLK));
 
@@ -84,7 +82,7 @@ void I2C_Init()
 
 // Send a START Condition
 //
-void I2C_Start()
+void I2C_Start(void)
 {
     // set both to high at the same time
     I2C_DDR &= ~ ((1 << I2C_DAT) | (1 << I2C_CLK));
@@ -99,7 +97,7 @@ void I2C_Start()
 
 // Send a STOP Condition
 //
-void I2C_Stop()
+void I2C_Stop(void)
 {
     I2C_CLOCK_HI();
     _delay_us(I2C_DELAY);
@@ -133,13 +131,13 @@ unsigned char I2C_Write(unsigned char c)
 #endif
 
 // Setleds for standard RGB
-void inline ws2812_setleds(struct cRGB *ledarray, uint16_t leds)
+void inline ws2812_setleds(LED_TYPE *ledarray, uint16_t leds)
 {
    // ws2812_setleds_pin(ledarray,leds, _BV(ws2812_pin));
    ws2812_setleds_pin(ledarray,leds, _BV(RGB_DI_PIN & 0xF));
 }
 
-void inline ws2812_setleds_pin(struct cRGB *ledarray, uint16_t leds, uint8_t pinmask)
+void inline ws2812_setleds_pin(LED_TYPE *ledarray, uint16_t leds, uint8_t pinmask)
 {
   // ws2812_DDRREG |= pinmask; // Enable DDR
   // new universal format (DDR)
@@ -150,17 +148,15 @@ void inline ws2812_setleds_pin(struct cRGB *ledarray, uint16_t leds, uint8_t pin
 }
 
 // Setleds for SK6812RGBW
-void inline ws2812_setleds_rgbw(struct cRGBW *ledarray, uint16_t leds)
+void inline ws2812_setleds_rgbw(LED_TYPE *ledarray, uint16_t leds)
 {
-  // ws2812_DDRREG |= _BV(ws2812_pin); // Enable DDR
-  // new universal format (DDR)
-  _SFR_IO8((RGB_DI_PIN >> 4) + 1) |= _BV(RGB_DI_PIN & 0xF);
-
-  ws2812_sendarray_mask((uint8_t*)ledarray,leds<<2,_BV(RGB_DI_PIN & 0xF));
 
   #ifdef RGBW_BB_TWI
+    uint8_t sreg_prev, twcr_prev;
+    sreg_prev=SREG;
+    twcr_prev=TWCR;
     cli();
-    TWCR = 0;
+    TWCR &= ~(1<<TWEN);
     I2C_Init();
     I2C_Start();
     I2C_Write(0x84);
@@ -169,14 +165,24 @@ void inline ws2812_setleds_rgbw(struct cRGBW *ledarray, uint16_t leds)
     uint8_t * data = (uint8_t*)ledarray;
     while (datlen--) {
       curbyte=*data++;
-      I2C_Write(curbyte % 0x10);
+      I2C_Write(curbyte);
     }
     I2C_Stop();
-    sei();
+    SREG=sreg_prev;
+    TWCR=twcr_prev;
   #endif
 
 
-  _delay_us(80);
+  // ws2812_DDRREG |= _BV(ws2812_pin); // Enable DDR
+  // new universal format (DDR)
+  _SFR_IO8((RGB_DI_PIN >> 4) + 1) |= _BV(RGB_DI_PIN & 0xF);
+
+  ws2812_sendarray_mask((uint8_t*)ledarray,leds<<2,_BV(RGB_DI_PIN & 0xF));
+
+
+  #ifndef RGBW_BB_TWI
+    _delay_us(80);
+  #endif
 }
 
 void ws2812_sendarray(uint8_t *data,uint16_t datlen)
@@ -258,7 +264,7 @@ void inline ws2812_sendarray_mask(uint8_t *data,uint16_t datlen,uint8_t maskhi)
   cli();
 
   while (datlen--) {
-    curbyte=(*data++) % 0x10;
+    curbyte=(*data++);
 
     asm volatile(
     "       ldi   %0,8  \n\t"