plugins/sixaxis: Factor out a calc_leds_bitmap() function

This is also in preparation of set_leds_sysfs().

diff --git a/plugins/sixaxis.c b/plugins/sixaxis.c
index 3a2cadf..1b7bb30 100644 (file)
--- a/plugins/sixaxis.c
+++ b/plugins/sixaxis.c
@@ -125,7 +125,25 @@ static int set_master_bdaddr(int fd, const bdaddr_t *bdaddr)
        return ret;
 }
 
-static void set_leds_hidraw(int fd, int number)
+static uint8_t calc_leds_bitmap(int number)
+{
+       uint8_t bitmap = 0;
+
+       /* TODO we could support up to 10 (1 + 2 + 3 + 4) */
+       if (number > 7)
+               return bitmap;
+
+       if (number > 4) {
+               bitmap |= 0x10;
+               number -= 4;
+       }
+
+       bitmap |= 0x01 << number;
+
+       return bitmap;
+}
+
+static void set_leds_hidraw(int fd, uint8_t leds_bitmap)
 {
        /*
         * the total time the led is active (0xff means forever)
@@ -148,16 +166,7 @@ static void set_leds_hidraw(int fd, int number)
        };
        int ret;
 
-       /* TODO we could support up to 10 (1 + 2 + 3 + 4) */
-       if (number > 7)
-               return;
-
-       if (number > 4) {
-               leds_report[10] |= 0x10;
-               number -= 4;
-       }
-
-       leds_report[10] |= 0x01 << number;
+       leds_report[10] = leds_bitmap;
 
        ret = write(fd, leds_report, sizeof(leds_report));
        if (ret == sizeof(leds_report))
@@ -173,6 +182,7 @@ static gboolean setup_leds(GIOChannel *channel, GIOCondition cond,
                                                        gpointer user_data)
 {
        int number = GPOINTER_TO_INT(user_data);
+       uint8_t bitmap;
        int fd;
 
        if (cond & (G_IO_HUP | G_IO_ERR | G_IO_NVAL))
@@ -181,7 +191,10 @@ static gboolean setup_leds(GIOChannel *channel, GIOCondition cond,
        DBG("number %d", number);
 
        fd = g_io_channel_unix_get_fd(channel);
-       set_leds_hidraw(fd, number);
+
+       bitmap = calc_leds_bitmap(number);
+       if (bitmap != 0)
+               set_leds_hidraw(fd, bitmap);
 
        return FALSE;
 }