Fix Phantom sleep LED.

3 files changed, 33 insertions, 10 deletions

diff --git a/keyboard/phantom/config.h b/keyboard/phantom/config.h
index 09f758cd05..6f5389336e 100644
--- a/keyboard/phantom/config.h
+++ b/keyboard/phantom/config.h
@@ -39,6 +39,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /* Set 0 if need no debouncing */
 #define DEBOUNCE    7
 
+/* Set LED brightness 0-255.
+ * This have no effect if sleep LED is enabled. */
+#define LED_BRIGHTNESS  250
+
 /* key combination for command */
 #define IS_COMMAND() ( \
     keyboard_report->mods == (MOD_BIT(KC_LSHIFT) | MOD_BIT(KC_RSHIFT)) \
diff --git a/keyboard/phantom/led.c b/keyboard/phantom/led.c
index 109004ba89..f4e9108f06 100644
--- a/keyboard/phantom/led.c
+++ b/keyboard/phantom/led.c
@@ -16,19 +16,34 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
 #include <avr/io.h>
-#include "stdint.h"
 #include "led.h"
 
 
 void led_set(uint8_t usb_led)
 {
-    if (!(usb_led & (1<<USB_LED_CAPS_LOCK)))
-        DDRB &= ~(1<<6);
-    else
+    if (usb_led & (1<<USB_LED_CAPS_LOCK))
+    {
+        // Output high.
         DDRB |= (1<<6);
+        PORTB |= (1<<6);
+    }
+    else
+    {
+        // Output low.
+        DDRB &= ~(1<<6);
+        PORTB &= ~(1<<6);
+    }
 
-    if (!(usb_led & (1<<USB_LED_SCROLL_LOCK)))
+    if (usb_led & (1<<USB_LED_SCROLL_LOCK))
+    {
+        // Output high.
         DDRB &= ~(1<<7);
+        PORTB |= (1<<7);
+    }
     else
-        DDRB |= (1<<7);
+    {
+        // Output low.
+        DDRB &= ~(1<<7);
+        PORTB &= ~(1<<7);
+    }
 }
diff --git a/keyboard/phantom/matrix.c b/keyboard/phantom/matrix.c
index c91c0d99ab..6c3ae49c3a 100644
--- a/keyboard/phantom/matrix.c
+++ b/keyboard/phantom/matrix.c
@@ -32,6 +32,7 @@ static void init_rows(void);
 static void unselect_cols(void);
 static void select_col(uint8_t col);
 
+#ifndef SLEEP_LED_ENABLE
 /* LEDs are on output compare pins OC1B OC1C
    This activates fast PWM mode on them.
    Prescaler 256 and 8-bit counter results in
@@ -51,12 +52,13 @@ void setup_leds(void)
     TCCR1B |=      // Timer control register 1B
         (1<<WGM12) | // Fast PWM 8-bit
         (1<<CS12);   // Prescaler 256
-    OCR1B = 250;    // Output compare register 1B
-    OCR1C = 250;    // Output compare register 1C
+    OCR1B = LED_BRIGHTNESS;    // Output compare register 1B
+    OCR1C = LED_BRIGHTNESS;    // Output compare register 1C
     // LEDs: LED_A -> PORTB6, LED_B -> PORTB7
-    DDRB  &= 0x3F;
-    PORTB &= 0x3F;
+    DDRB  |= (1<<6) | (1<<7);
+    PORTB  &= ~((1<<6) | (1<<7));
 }
+#endif
 
 inline
 uint8_t matrix_rows(void)
@@ -79,7 +81,9 @@ void matrix_init(void)
     // initialize row and col
     unselect_cols();
     init_rows();
+#ifndef SLEEP_LED_ENABLE
     setup_leds();
+#endif
 
     // initialize matrix state: all keys off
     for (uint8_t i = 0; i < MATRIX_ROWS; i++)  {