release/src/router/shared/led.c

   1 /*
   2
   3         Tomato Firmware
   4         Copyright (C) 2006-2009 Jonathan Zarate
   5
   6 */
   7 #include <stdio.h>
   8 #include <stdlib.h>
   9 #include <string.h>
  10 #include <unistd.h>
  11 #include <syslog.h>
  12 #include <sys/types.h>
  13 #include <sys/stat.h>
  14 #include <fcntl.h>
  15 #include <bcmnvram.h>
  16
  17 #include "utils.h"
  18 #include "shutils.h"
  19 #include "shared.h"
  20
  21
  22 const char *led_names[] = { "wlan", "diag", "white", "amber", "dmz", "aoss", "bridge", "usb", "5g"};
  23 const char *led_modes[] = { "Off", "On", "Blink", "Probe"};
  24
  25 #ifdef LINUX26
  26 #define GPIO_IOCTL
  27 #endif
  28
  29 // --- move begin ---
  30 #ifdef GPIO_IOCTL
  31
  32 #include <sys/ioctl.h>
  33 #include <linux_gpio.h>
  34
  35 static int _gpio_ioctl(int f, int gpioreg, unsigned int mask, unsigned int val)
  36 {
  37         struct gpio_ioctl gpio;
  38
  39         gpio.val = val;
  40         gpio.mask = mask;
  41
  42         if (ioctl(f, gpioreg, &gpio) < 0) {
  43                 _dprintf("Invalid gpioreg %d\n", gpioreg);
  44                 return -1;
  45         }
  46         return (gpio.val);
  47 }
  48
  49 static int _gpio_open()
  50 {
  51         int f = open("/dev/gpio", O_RDWR);
  52         if (f < 0)
  53                 _dprintf ("Failed to open /dev/gpio\n");
  54         return f;
  55 }
  56
  57 int gpio_open(uint32_t mask)
  58 {
  59         uint32_t bit;
  60         int i;
  61         int f = _gpio_open();
  62
  63         if ((f >= 0) && mask) {
  64                 for (i = 0; i <= 15; i++) {
  65                         bit = 1 << i;
  66                         if ((mask & bit) == bit) {
  67                                 _gpio_ioctl(f, GPIO_IOC_RESERVE, bit, bit);
  68                                 _gpio_ioctl(f, GPIO_IOC_OUTEN, bit, 0);
  69                         }
  70                 }
  71                 close(f);
  72                 f = _gpio_open();
  73         }
  74
  75         return f;
  76 }
  77
  78 void gpio_write(uint32_t bit, int en)
  79 {
  80         int f;
  81
  82         if ((f = gpio_open(0)) < 0) return;
  83
  84         _gpio_ioctl(f, GPIO_IOC_RESERVE, bit, bit);
  85         _gpio_ioctl(f, GPIO_IOC_OUTEN, bit, bit);
  86         _gpio_ioctl(f, GPIO_IOC_OUT, bit, en ? bit : 0);
  87         close(f);
  88 }
  89
  90 uint32_t _gpio_read(int f)
  91 {
  92         uint32_t r;
  93 //      r = _gpio_ioctl(f, GPIO_IOC_IN, 0xFFFF, 0);
  94         r = _gpio_ioctl(f, GPIO_IOC_IN, 0x07FF, 0);
  95         if (r < 0) r = ~0;
  96         return r;
  97 }
  98
  99 uint32_t gpio_read(void)
 100 {
 101         int f;
 102         uint32_t r;
 103
 104         if ((f = gpio_open(0)) < 0) return ~0;
 105         r = _gpio_read(f);
 106         close(f);
 107         return r;
 108 }
 109
 110 #else
 111
 112 int gpio_open(uint32_t mask)
 113 {
 114         int f = open(DEV_GPIO(in), O_RDONLY|O_SYNC);
 115         if (f < 0)
 116                 _dprintf ("Failed to open %s\n", DEV_GPIO(in));
 117         return f;
 118 }
 119
 120 void gpio_write(uint32_t bit, int en)
 121 {
 122         int f;
 123         uint32_t r;
 124
 125         if ((f = open(DEV_GPIO(control), O_RDWR)) < 0) return;
 126         read(f, &r, sizeof(r));
 127         r &= ~bit;
 128         write(f, &r, sizeof(r));
 129         close(f);
 130
 131         if ((f = open(DEV_GPIO(outen), O_RDWR)) < 0) return;
 132         read(f, &r, sizeof(r));
 133         r |= bit;
 134         write(f, &r, sizeof(r));
 135         close(f);
 136
 137         if ((f = open(DEV_GPIO(out), O_RDWR)) < 0) return;
 138         read(f, &r, sizeof(r));
 139         if (en) r |= bit;
 140                 else r &= ~bit;
 141         write(f, &r, sizeof(r));
 142         close(f);
 143 }
 144
 145 uint32_t _gpio_read(int f)
 146 {
 147         uint32_t v;
 148         return (read(f, &v, sizeof(v)) == sizeof(v)) ? v : ~0;
 149 }
 150
 151 uint32_t gpio_read(void)
 152 {
 153         int f;
 154         uint32_t r;
 155
 156         if ((f = open(DEV_GPIO(in), O_RDONLY)) < 0) return ~0;
 157         r = _gpio_read(f);
 158         close(f);
 159         return r;
 160 }
 161
 162 #endif
 163
 164 int nvget_gpio(const char *name, int *gpio, int *inv)
 165 {
 166         char *p;
 167         uint32_t n;
 168
 169         if (((p = nvram_get(name)) != NULL) && (*p)) {
 170                 n = strtoul(p, NULL, 0);
 171                 if ((n & 0xFFFFFF70) == 0) {
 172                         *gpio = (n & 15);
 173                         *inv = ((n & 0x80) != 0);
 174                         return 1;
 175                 }
 176         }
 177         return 0;
 178 }
 179 // --- move end ---
 180 // Routine to write to shift register
 181 // Note that the controls are active low, but input as high = on
 182 void gpio_write_shiftregister(unsigned int led_status, int clk, int data, int max_shifts)
 183 {
 184         int i;
 185
 186         gpio_write(1 << data, 1);       /* set data to 1 to start (disable) */
 187         gpio_write(1 << clk, 0);        /* and clear clock ... */
 188
 189         for (i = max_shifts; i >= 0; i--) {
 190                 if (led_status & (1 << i))
 191                         gpio_write(1 << data, 0);       /* on, pull low (active low) */
 192                 else
 193                         gpio_write(1 << data, 1);       /* off, pull high (active low) */
 194
 195                 gpio_write(1 << clk, 1);        /* pull high to trigger */
 196                 gpio_write(1 << clk, 0);        /* reset to low -> finish clock cycle*/
 197         }
 198 }
 199
 200 /* strBits:  convert binary value to string (binary file representation) */
 201 char strConvert[33];
 202 char * strBits(int input, int binarySize)
 203 {
 204
 205         int i;
 206
 207         if (binarySize > 0) {
 208
 209                 if (binarySize > 32)
 210                         binarySize = 32;
 211
 212                 for(i = 0; i < binarySize ; i++) {
 213                         if (input & (1 << ((binarySize-1)-i)))
 214                                 strConvert[i] = '1';
 215                         else
 216                                 strConvert[i] = '0';
 217                 }
 218
 219                 strConvert[binarySize] = '\0';
 220                 return (char *)strConvert;
 221
 222         } else
 223                 return (char *)NULL;
 224 }
 225
 226 /* bwq518 */
 227 /* return 0 success, 1 fail */
 228 /* debug == 1, output to syslog */
 229 int led_bit(int b, int mode, int debug)
 230 {
 231         FILE *fileExtGPIOstatus;                // For WNDR4000, keep track of extended bit status (shift register), as cannot read from HW!
 232         unsigned int intExtendedLEDStatus;      // Status of Extended LED's (shift register on WNDR4000) ... and WNDR3700v3, it's the same!
 233
 234         if ((mode == LED_ON) || (mode == LED_OFF)) {
 235                 if (b < 16) {
 236                         // Read bit-mask from file, for tracking / updates (as this process is called clean each LED update, so cannot use static variable!)
 237                         if (!(fileExtGPIOstatus = fopen("/tmp/.ext_led_value", "rb"))) {
 238                                 fscanf(fileExtGPIOstatus, "Shift Register Status: 0x%x\n", &intExtendedLEDStatus);
 239                                 fclose(fileExtGPIOstatus);
 240                                 return 1;
 241                                 if (debug) syslog(LOG_INFO, "Netgear Shift Register (do_led): Read Shift Register status from file, intExtendedLEDStatus = %s\n", strBits(intExtendedLEDStatus, 8));
 242                         } else {
 243                                 // Read Error (tracking file) - set all LED's to off
 244                                 syslog(LOG_INFO, "Netgear Shift Register (do_led): Error Reading /tmp/.ext_led_value, set state to all OFF\n");
 245                                 intExtendedLEDStatus = 0x00;
 246                         }
 247
 248                         if (mode == LED_ON) {
 249                                 // Bitwise OR, turn corresponding bit on
 250                                 intExtendedLEDStatus |= (1 << b);
 251                                 if (debug) syslog(LOG_INFO, "Netgear Shift Register (do_led): Mode = LED_ON (%d), Bitwise OR = %s\n", mode, strBits((1 << b), 8));
 252                         } else {
 253                                 // Bitwise AND, with bitwise inverted shift ... so turn bit off
 254                                 intExtendedLEDStatus &= (~(1 << b));
 255                                 if(debug) syslog(LOG_INFO, "Netgear Shift Register (do_led): Mode = LED_OFF (%d), Bitwise AND = %s\n", mode, strBits((~(1 << b)), 8));
 256                         }
 257
 258                         // And write to LEDs (Shift Register)
 259                         if(debug) syslog(LOG_INFO, "Netgear Shift Register (do_led): Writing to Shift Register, intExtendedLEDStatus = %s\n", strBits(intExtendedLEDStatus, 8));
 260                         gpio_write_shiftregister(intExtendedLEDStatus, 7, 6, 7);
 261                         // Write bit-mask to file, for tracking / updates (as this process is called clean each LED update, so cannot use static variable!)
 262                         if (!(fileExtGPIOstatus = fopen("/tmp/.ext_led_value", "wb"))) {
 263                                 fprintf(fileExtGPIOstatus, "Shift Register Status: 0x%x\n", intExtendedLEDStatus);
 264                                 fprintf(fileExtGPIOstatus, "Shift Register Status: 0b%s\n", strBits(intExtendedLEDStatus, 8));
 265                                 fclose(fileExtGPIOstatus);
 266                         }
 267                 }
 268         }
 269         return 0;
 270 }
 271 int do_led(int which, int mode)
 272 {
 273 //                                  WLAN  DIAG  WHITE AMBER DMZ   AOSS  BRIDG MYST/USB 5G
 274 //                                  ----- ----- ----- ----- ----- ----- ----- -----    --
 275         static int wrt54g[]     = { 255,  1,    2,    3,    7,    255,  255,  255,    255};
 276         static int wrtsl[]      = { 255,  1,    5,    7,    0,    255,  255,  255,    255};
 277         static int whrg54[]     = { 2,    7,    255,  255,  255,  6,    1,    3  ,    255};
 278         static int wbr2g54[]    = { 255,  -1,   255,  255,  255,  -6,   255,  255,    255};
 279         static int wzrg54[]     = { 2,    7,    255,  255,  255,  6,    255,  255,    255};
 280         static int wr850g1[]    = { 7,    3,    255,  255,  255,  255,  255,  255,    255};
 281         static int wr850g2[]    = { 0,    1,    255,  255,  255,  255,  255,  255,    255};
 282         static int wtr54gs[]    = { 1,    -1,   255,  255,  255,  255,  255,  255,    255};
 283         static int dir320[]     = { -99,   1,     4,    3,  255,  255,  255,   -5,    255};
 284         static int h618b[]      = { 255,  -1,   255,  255,  255,   -5,   -3,   -4,    255};
 285         static int wl1600gl[]   = { 1,    -5,     0,  255,  255,  2,    255,  255,    255};
 286         static int wrt310nv1[]  = { 255,   1,     9,    3,  255,  255,  255,  255,    255};
 287         static int wrt160nv1[]  = { 255,   1,     5,    3,  255,  255,  255,  255,    255};
 288 #ifdef CONFIG_BCMWL5
 289         static int wnr3500[]    = { 255, 255,     2,  255,  255,   -1,  255,  255,    255};
 290         static int wnr2000v2[]  = { 255, 255,   255,  255,  255,   -7,  255,  255,    255};
 291         static int wndr4000[]   = {   3,   1,     0,    1,  255,    6,  255,    5,      4};
 292         static int wndr3400[]   = {  -9,  -7,    -3,   -7,  255,   255, 255,    2,    -99}; // Note: 5 = Switch, 4 = Reset button, 8 = SES button
 293         static int f7d[]        = { 255, 255,   255,  255,   12,   13,  255,   14,    255};
 294         static int wrt160nv3[]  = { 255,   1,     4,    2,  255,  255,  255,  255,    255};
 295         static int e900[]       = { 255,  -6,     8,  255,  255,  255,  255,  255,    255};
 296         static int e1000v2[]    = { 255,  -6,     8,    7,  255,  255,  255,  255,    255};
 297         static int e3200[]      = { 255,  -3,   255,  255,  255,  255,  255,  255,    255};
 298         static int wrt320n[]    = { 255,   2,     3,    4,  255,  255,  255,  255,    255};
 299         static int wrt610nv2[]  = { 255,   5,     3,    0,  255,  255,  255,   -7,    255};
 300         static int e4200[]      = { 255,   5,    -3,  255,  255,  255,  255,  255,    255};
 301         static int rtn10u[]     = { 255, 255,   255,  255,  255,   -7,  255,   -8,    255};
 302         static int rtn10p[]     = { 255,  -6,   255,  255,  255,   -7,  255,  255,    255};
 303         static int rtn12b1[]    = {  -5, 255,   255,  255,  255,  255,  255,  225,    255};
 304         static int rtn15u[]     = {   1, 255,     3,  255,  255,  255,  255,   -9,    255};
 305         static int rtn53[]      = {   0, -17,   255,  255,  255,  255,  255,  255,    255};
 306         static int l600n[]      = { 255, 255,   255,  255,  255,   -7,  255,   -8,    255};
 307         static int dir620c1[]   = {  -6,  -8,   255,  255,  255,   -7,  255,  255,    255};
 308         static int rtn66u[]     = { 255, -12,   255,  255,  255,  255,  255,   15,     13};
 309         static int w1800r[]     = { 255, -13,   255,  255,  255,  255,  255,  -12,     -5};
 310         static int d1800h[]     = { -12, -13,     8,  255,  255,  -10,  255,   15,     11};
 311         static int tdn6[]       = { 255,  -6,     8,  255,  255,  255,  255,  255,    255};
 312         static int tdn60[]      = { 255,  -6,     8,  255,  255,  255,  255,    9,    255};
 313         static int r6300v1[]    = {  11,   3,   255,  255,  255,  255,  255,    8,     11};
 314         static int wndr4500[]   = {   9,   3,     2,    3,  255,  255,  255,   14,     11};
 315 //                                 WLAN  DIAG  WHITE AMBER DMZ   AOSS  BRIDG MYST/USB 5G
 316
 317 #endif
 318
 319         char s[16];
 320         int n;
 321         int b = 255, c = 255;
 322         int ret = 255;
 323
 324         if ((which < 0) || (which >= LED_COUNT)) return ret;
 325
 326         switch (nvram_match("led_override", "1") ? MODEL_UNKNOWN : get_model()) {
 327         case MODEL_WRT54G:
 328                 if (check_hw_type() == HW_BCM4702) {
 329                         // G v1.x
 330                         if ((which != LED_DIAG) && (which != LED_DMZ)) return ret;
 331                         b = (which == LED_DMZ) ? 1 : 4;
 332                         if (mode != LED_PROBE) {
 333                                 if (f_read_string("/proc/sys/diag", s, sizeof(s)) > 0) {
 334                                         n = atoi(s);
 335                                         sprintf(s, "%u", mode ? (n | b) : (n & ~b));
 336                                         f_write_string("/proc/sys/diag", s, 0, 0);
 337                                 }
 338                         }
 339                         return b;
 340                 }
 341                 switch (which) {
 342                 case LED_AMBER:
 343                 case LED_WHITE:
 344                         if (!supports(SUP_WHAM_LED)) return ret;
 345                         break;
 346                 }
 347                 b = wrt54g[which];
 348                 break;
 349         case MODEL_WTR54GS:
 350                 b = wtr54gs[which];
 351                 break;
 352         case MODEL_WRTSL54GS:
 353                 b = wrtsl[which];
 354                 break;
 355         case MODEL_WHRG54S:
 356         case MODEL_WHRHPG54:
 357         case MODEL_WHRG125:
 358                 b = whrg54[which];
 359                 break;
 360         case MODEL_WZRG54:
 361         case MODEL_WZRHPG54:
 362         case MODEL_WZRRSG54:
 363         case MODEL_WZRRSG54HP:
 364         case MODEL_WVRG54NF:
 365         case MODEL_WHR2A54G54:
 366         case MODEL_WHR3AG54:
 367         case MODEL_WZRG108:
 368                 b = wzrg54[which];
 369                 break;
 370 /*
 371         case MODEL_WHR2A54G54:
 372                 if (which != LED_DIAG) return ret;
 373                 b = 7;
 374                 break;
 375 */
 376         case MODEL_WBRG54:
 377                 if (which != LED_DIAG) return ret;
 378                 b = 7;
 379                 break;
 380         case MODEL_WBR2G54:
 381                 b = wbr2g54[which];
 382                 break;
 383         case MODEL_WR850GV1:
 384                 b = wr850g1[which];
 385                 break;
 386         case MODEL_WR850GV2:
 387         case MODEL_WR100:
 388                 b = wr850g2[which];
 389                 break;
 390         case MODEL_WL500GP:
 391                 if (which != LED_DIAG) return ret;
 392                 b = -1; // power light
 393                 break;
 394         case MODEL_WL500W:
 395                 if (which != LED_DIAG) return ret;
 396                 b = -5; // power light
 397                 break;
 398         case MODEL_DIR320:
 399                 b = dir320[which];
 400                 break;
 401         case MODEL_H618B:
 402                 b = h618b[which];
 403                 break;
 404         case MODEL_WL1600GL:
 405                 b = wl1600gl[which];
 406                 break;
 407         case MODEL_WL500GPv2:
 408         case MODEL_WL500GD:
 409         case MODEL_WL520GU:
 410         case MODEL_WL330GE:
 411                 if (which != LED_DIAG) return ret;
 412                 b = -99;        // Invert power light as diag indicator
 413                 break;
 414 #ifdef CONFIG_BCMWL5
 415         case MODEL_RTN12:
 416                 if (which != LED_DIAG) return ret;
 417                 b = -2; // power light
 418                 break;
 419         case MODEL_RTN10:
 420         case MODEL_RTN16:
 421                 if (which != LED_DIAG) return ret;
 422                 b = -1; // power light
 423                 break;
 424         case MODEL_RTN15U:
 425                 b = rtn15u[which];
 426                 break;
 427         case MODEL_RTN53:
 428         case MODEL_RTN53A1:
 429                 b = rtn53[which];
 430                 break;
 431         case MODEL_RTN66U:
 432                 b = rtn66u[which];
 433                 break;
 434         case MODEL_W1800R:
 435         case MODEL_TDN80:
 436                 b = w1800r[which];
 437                 break;
 438         case MODEL_D1800H:
 439                 if (which == LED_DIAG) {
 440                         // power led gpio: 0x02 - white, 0x13 - red
 441                         b = (mode) ? 13 : 2;
 442                         c = (mode) ? 2 : 13;
 443                 } else
 444                         b = d1800h[which];
 445                 break;
 446         case MODEL_WNR3500L:
 447         case MODEL_WNR3500LV2:
 448                 if (which == LED_DIAG) {
 449                         // power led gpio: 0x03 - green, 0x07 - amber
 450                         b = (mode) ? 7 : 3;
 451                         c = (mode) ? 3 : 7;
 452                 } else
 453                         b = wnr3500[which];
 454                 break;
 455         case MODEL_WNDR4500:
 456         case MODEL_WNDR4500V2:
 457                 if (which == LED_DIAG) {
 458                         // power led gpio: 0x102 - green, 0x103 - amber
 459                         b = (mode) ? 3 : -2;
 460                         c = (mode) ? -2 : 3;
 461                 } else {
 462                         b = wndr4500[which];
 463                 }
 464                 break;
 465         case MODEL_R6300V1:
 466                 b = r6300v1[which];
 467                 break;
 468         case MODEL_WNR2000v2:
 469                 if (which == LED_DIAG) {
 470                         // power led gpio: 0x01 - green, 0x02 - amber
 471                         b = (mode) ? 2 : 1;
 472                         c = (mode) ? 1 : 2;
 473                 } else
 474                         b = wnr2000v2[which];
 475                 break;
 476         case MODEL_WNDR4000:
 477         case MODEL_WNDR3700v3:
 478                 // Special Case, shift register control ... so write accordingly. Syslog below for debugging, turn back on if needed.
 479                 b = wndr4000[which];
 480                 //syslog(LOG_INFO, "WNDR4000 Shift Register (do_led): Bit Name = %s, which = %d, b = %d, Mode = %s\n", led_names[which], which, b, led_modes[mode]);
 481                 led_bit(b, mode, 0);
 482                 return b;
 483                 break;
 484         case MODEL_WNDR3400:
 485         case MODEL_WNDR3400v2:
 486                 b = wndr3400[which];
 487                 break;
 488         case MODEL_F7D3301:
 489         case MODEL_F7D3302:
 490         case MODEL_F7D4301:
 491         case MODEL_F7D4302:
 492         case MODEL_F5D8235v3:
 493                 if (which == LED_DIAG) {
 494                         // power led gpio: 10 - green, 11 - red
 495                         b = (mode) ? 11 : -10;
 496                         c = (mode) ? -10 : 11;
 497                 } else
 498                         b = f7d[which];
 499                 break;
 500         case MODEL_E1000v2:
 501                 b = e1000v2[which];
 502                 break;
 503         case MODEL_E900:
 504         case MODEL_E1500:
 505         case MODEL_E1550:
 506         case MODEL_E2500:
 507                 b = e900[which];
 508                 break;
 509         case MODEL_E3200:
 510                 b = e3200[which];
 511                 break;
 512         case MODEL_WRT160Nv3:
 513                 b = wrt160nv3[which];
 514                 break;
 515         case MODEL_WRT320N:
 516                 b = wrt320n[which];
 517                 break;
 518         case MODEL_WRT610Nv2:
 519                 b = wrt610nv2[which];
 520                 break;
 521         case MODEL_E4200:
 522                 b = e4200[which];
 523                 break;
 524         case MODEL_RTN10U:
 525                 b = rtn10u[which];
 526                 break;
 527         case MODEL_RTN10P:
 528                 b = rtn10p[which];
 529                 break;
 530         case MODEL_RTN12B1:
 531                 b = rtn12b1[which];
 532                 break;
 533         case MODEL_L600N:
 534                 b = l600n[which];
 535                 break;
 536         case MODEL_DIR620C1:
 537                 b = dir620c1[which];
 538         case MODEL_TDN60:
 539                 b = tdn60[which];
 540         case MODEL_TDN6:
 541                 b = tdn6[which];
 542                 break;
 543 #endif
 544 /*
 545         case MODEL_RT390W:
 546                 break;
 547 */
 548         case MODEL_MN700:
 549                 if (which != LED_DIAG) return ret;
 550                 b = 6;
 551                 break;
 552         case MODEL_WLA2G54L:
 553                 if (which != LED_DIAG) return ret;
 554                 b = 1;
 555                 break;
 556         case MODEL_WRT300N:
 557                 if (which != LED_DIAG) return ret;
 558                 b = 1;
 559                 break;
 560         case MODEL_WRT310Nv1:
 561                 b = wrt310nv1[which];
 562                 break;
 563         case MODEL_WRT160Nv1:
 564                 b = wrt160nv1[which];
 565                 break;
 566         default:
 567                 sprintf(s, "led_%s", led_names[which]);
 568                 if (nvget_gpio(s, &b, &n)) {
 569                         if ((mode != LED_PROBE) && (n)) mode = !mode;
 570                         ret = (n) ? b : ((b) ? -b : -99);
 571                         goto SET;
 572                 }
 573                 return ret;
 574         }
 575
 576         ret = b;
 577         if (b < 0) {
 578                 if (b == -99) b = 0; // -0 substitute
 579                         else b = -b;
 580         }
 581         else if (mode != LED_PROBE) {
 582                 mode = !mode;
 583         }
 584
 585 SET:
 586         if (b < 16) {
 587                 if (mode != LED_PROBE) {
 588                         gpio_write(1 << b, mode);
 589
 590                         if (c < 0) {
 591                                 if (c == -99) c = 0;
 592                                 else c = -c;
 593                         }
 594                         else mode = !mode;
 595                         if (c < 16) gpio_write(1 << c, mode);
 596                 }
 597         }
 598
 599         return ret;
 600 }