From e60ddbac5a55d21d43dafabf32983414c3a58bcd Mon Sep 17 00:00:00 2001
From: Kristian Lauszus <lauszus@gmail.com>
Date: Wed, 9 May 2012 18:38:38 +0200
Subject: [PATCH] Added setLedToggle()

---
 PS3BT.cpp    | 34 ++++++++++++----------------------
 PS3BT.h      |  1 +
 keywords.txt |  1 +
 3 files changed, 14 insertions(+), 22 deletions(-)

diff --git a/PS3BT.cpp b/PS3BT.cpp
index f64138ad..3e5f8107 100644
--- a/PS3BT.cpp
+++ b/PS3BT.cpp
@@ -406,17 +406,13 @@ uint8_t PS3BT::Poll()
 	return 0;
 }
 void PS3BT::setBdaddr(uint8_t* BDADDR)
-{
-    /* Store the bluetooth address */
-    for(uint8_t i = 0; i <6;i++)
-        my_bdaddr[i] = BDADDR[i];
-    
+{    
     /* Set the internal bluetooth address */             
     uint8_t buf[8];            
     buf[0] = 0x01;
     buf[1] = 0x00;
     for (uint8_t i = 0; i < 6; i++)
-        buf[i+2] = my_bdaddr[5 - i];//Copy into buffer, has to be written reversed
+        buf[i+2] = BDADDR[5 - i];//Copy into buffer, has to be written reversed
     
     //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data)
     pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL);      
@@ -433,10 +429,6 @@ void PS3BT::setBdaddr(uint8_t* BDADDR)
 }
 void PS3BT::setMoveBdaddr(uint8_t* BDADDR)
 {
-    /* Store the bluetooth address */
-    for(uint8_t i = 0; i <6;i++)
-        my_bdaddr[i] = BDADDR[i];
-    
 	/* Set the internal bluetooth address */             
     uint8_t buf[11];
     buf[0] = 0x05;
@@ -446,7 +438,7 @@ void PS3BT::setMoveBdaddr(uint8_t* BDADDR)
     buf[10] = 0x12;    
     
     for (uint8_t i = 0; i < 6; i++)
-        buf[i + 1] = my_bdaddr[i];
+        buf[i + 1] = BDADDR[i];
     
     //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x05), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data)
     pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL);   
@@ -798,7 +790,7 @@ void PS3BT::HCI_task()
                 hci_state = HCI_LOCAL_VERSION_STATE;                                
             }
             break;
-                        
+            
         case HCI_LOCAL_VERSION_STATE:
             if (hci_cmd_complete) 
             {
@@ -1591,19 +1583,17 @@ void PS3BT::setRumbleOn(Rumble mode)
 }
 void PS3BT::setLedOff(LED a)
 {
-    //check if LED is already off
-    if ((uint8_t)((uint8_t)(((uint16_t)a << 1) & HIDBuffer[11])) != 0)
-    {
-        //set the LED into the write buffer
-        HIDBuffer[11] = (uint8_t)((uint8_t)(((uint16_t)a & 0x0f) << 1) ^ HIDBuffer[11]);
-        
-        HID_Command(HIDBuffer, HID_BUFFERSIZE);
-    }            
+    HIDBuffer[11] &= ~((uint8_t)(((uint16_t)a & 0x0f) << 1));    
+    HID_Command(HIDBuffer, HID_BUFFERSIZE);               
 }
 void PS3BT::setLedOn(LED a)
 {
-    HIDBuffer[11] = (uint8_t)((uint8_t)(((uint16_t)a & 0x0f) << 1) | HIDBuffer[11]);
-    
+    HIDBuffer[11] |= (uint8_t)(((uint16_t)a & 0x0f) << 1);    
+    HID_Command(HIDBuffer, HID_BUFFERSIZE);            
+}
+void PS3BT::setLedToggle(LED a)
+{
+    HIDBuffer[11] ^= (uint8_t)(((uint16_t)a & 0x0f) << 1);    
     HID_Command(HIDBuffer, HID_BUFFERSIZE);            
 }
 void PS3BT::enable_sixaxis()//Command used to enable the Dualshock 3 and Navigation controller to send data via USB
diff --git a/PS3BT.h b/PS3BT.h
index b228a91c..b2983b7c 100644
--- a/PS3BT.h
+++ b/PS3BT.h
@@ -352,6 +352,7 @@ public:
     void setRumbleOn(Rumble mode);
     void setLedOff(LED a);
     void setLedOn(LED a);    
+    void setLedToggle(LED a);
     /* Commands for Motion controller only */    
     void moveSetBulb(uint8_t r, uint8_t g, uint8_t b);//Use this to set the Color using RGB values
     void moveSetBulb(Colors color);//Use this to set the Color using the predefined colors in "enum Colors"
diff --git a/keywords.txt b/keywords.txt
index 378fc91a..8b11fbcf 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -28,6 +28,7 @@ setRumbleOff	KEYWORD2
 setRumbleOn	KEYWORD2
 setLedOff	KEYWORD2
 setLedOn	KEYWORD2
+setLedToggle	KEYWORD2
 moveSetBulb	KEYWORD2
 moveSetRumble	KEYWORD2