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 dir865l[]    = { 255, -99,     2,    1,  255,    3,  255,  255,    255};
 313         static int tdn60[]      = { 255,  -6,     8,  255,  255,  255,  255,    9,    255};
 314         static int r6300v1[]    = {  11,   3,   255,  255,  255,  255,  255,    8,     11};
 315         static int wndr4500[]   = {   9,   3,     2,    3,  255,  255,  255,   14,     11};
 316 //                                 WLAN  DIAG  WHITE AMBER DMZ   AOSS  BRIDG MYST/USB 5G
 317
 318 #endif
 319
 320         char s[16];
 321         int n;
 322         int b = 255, c = 255;
 323         int ret = 255;
 324
 325         if ((which < 0) || (which >= LED_COUNT)) return ret;
 326
 327         switch (nvram_match("led_override", "1") ? MODEL_UNKNOWN : get_model()) {
 328         case MODEL_WRT54G:
 329                 if (check_hw_type() == HW_BCM4702) {
 330                         // G v1.x
 331                         if ((which != LED_DIAG) && (which != LED_DMZ)) return ret;
 332                         b = (which == LED_DMZ) ? 1 : 4;
 333                         if (mode != LED_PROBE) {
 334                                 if (f_read_string("/proc/sys/diag", s, sizeof(s)) > 0) {
 335                                         n = atoi(s);
 336                                         sprintf(s, "%u", mode ? (n | b) : (n & ~b));
 337                                         f_write_string("/proc/sys/diag", s, 0, 0);
 338                                 }
 339                         }
 340                         return b;
 341                 }
 342                 switch (which) {
 343                 case LED_AMBER:
 344                 case LED_WHITE:
 345                         if (!supports(SUP_WHAM_LED)) return ret;
 346                         break;
 347                 }
 348                 b = wrt54g[which];
 349                 break;
 350         case MODEL_WTR54GS:
 351                 b = wtr54gs[which];
 352                 break;
 353         case MODEL_WRTSL54GS:
 354                 b = wrtsl[which];
 355                 break;
 356         case MODEL_WHRG54S:
 357         case MODEL_WHRHPG54:
 358         case MODEL_WHRG125:
 359                 b = whrg54[which];
 360                 break;
 361         case MODEL_WZRG54:
 362         case MODEL_WZRHPG54:
 363         case MODEL_WZRRSG54:
 364         case MODEL_WZRRSG54HP:
 365         case MODEL_WVRG54NF:
 366         case MODEL_WHR2A54G54:
 367         case MODEL_WHR3AG54:
 368         case MODEL_WZRG108:
 369                 b = wzrg54[which];
 370                 break;
 371 /*
 372         case MODEL_WHR2A54G54:
 373                 if (which != LED_DIAG) return ret;
 374                 b = 7;
 375                 break;
 376 */
 377         case MODEL_WBRG54:
 378                 if (which != LED_DIAG) return ret;
 379                 b = 7;
 380                 break;
 381         case MODEL_WBR2G54:
 382                 b = wbr2g54[which];
 383                 break;
 384         case MODEL_WR850GV1:
 385                 b = wr850g1[which];
 386                 break;
 387         case MODEL_WR850GV2:
 388         case MODEL_WR100:
 389                 b = wr850g2[which];
 390                 break;
 391         case MODEL_WL500GP:
 392                 if (which != LED_DIAG) return ret;
 393                 b = -1; // power light
 394                 break;
 395         case MODEL_WL500W:
 396                 if (which != LED_DIAG) return ret;
 397                 b = -5; // power light
 398                 break;
 399         case MODEL_DIR320:
 400                 b = dir320[which];
 401                 break;
 402         case MODEL_H618B:
 403                 b = h618b[which];
 404                 break;
 405         case MODEL_WL1600GL:
 406                 b = wl1600gl[which];
 407                 break;
 408         case MODEL_WL500GPv2:
 409         case MODEL_WL500GD:
 410         case MODEL_WL520GU:
 411         case MODEL_WL330GE:
 412                 if (which != LED_DIAG) return ret;
 413                 b = -99;        // Invert power light as diag indicator
 414                 break;
 415 #ifdef CONFIG_BCMWL5
 416         case MODEL_RTN12:
 417                 if (which != LED_DIAG) return ret;
 418                 b = -2; // power light
 419                 break;
 420         case MODEL_RTN10:
 421         case MODEL_RTN16:
 422                 if (which != LED_DIAG) return ret;
 423                 b = -1; // power light
 424                 break;
 425         case MODEL_RTN15U:
 426                 b = rtn15u[which];
 427                 break;
 428         case MODEL_RTN53:
 429         case MODEL_RTN53A1:
 430                 b = rtn53[which];
 431                 break;
 432         case MODEL_RTN66U:
 433                 b = rtn66u[which];
 434                 break;
 435         case MODEL_DIR865L:
 436                 b = dir865l[which];
 437                 break;
 438         case MODEL_W1800R:
 439         case MODEL_TDN80:
 440                 b = w1800r[which];
 441                 break;
 442         case MODEL_D1800H:
 443                 if (which == LED_DIAG) {
 444                         // power led gpio: 0x02 - white, 0x13 - red
 445                         b = (mode) ? 13 : 2;
 446                         c = (mode) ? 2 : 13;
 447                 } else
 448                         b = d1800h[which];
 449                 break;
 450         case MODEL_WNR3500L:
 451         case MODEL_WNR3500LV2:
 452                 if (which == LED_DIAG) {
 453                         // power led gpio: 0x03 - green, 0x07 - amber
 454                         b = (mode) ? 7 : 3;
 455                         c = (mode) ? 3 : 7;
 456                 } else
 457                         b = wnr3500[which];
 458                 break;
 459         case MODEL_WNDR4500:
 460         case MODEL_WNDR4500V2:
 461                 if (which == LED_DIAG) {
 462                         // power led gpio: 0x102 - green, 0x103 - amber
 463                         b = (mode) ? 3 : -2;
 464                         c = (mode) ? -2 : 3;
 465                 } else {
 466                         b = wndr4500[which];
 467                 }
 468                 break;
 469         case MODEL_R6300V1:
 470                 b = r6300v1[which];
 471                 break;
 472         case MODEL_WNR2000v2:
 473                 if (which == LED_DIAG) {
 474                         // power led gpio: 0x01 - green, 0x02 - amber
 475                         b = (mode) ? 2 : 1;
 476                         c = (mode) ? 1 : 2;
 477                 } else
 478                         b = wnr2000v2[which];
 479                 break;
 480         case MODEL_WNDR4000:
 481         case MODEL_WNDR3700v3:
 482                 // Special Case, shift register control ... so write accordingly. Syslog below for debugging, turn back on if needed.
 483                 b = wndr4000[which];
 484                 //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]);
 485                 led_bit(b, mode, 0);
 486                 return b;
 487                 break;
 488         case MODEL_WNDR3400:
 489         case MODEL_WNDR3400v2:
 490                 b = wndr3400[which];
 491                 break;
 492         case MODEL_F7D3301:
 493         case MODEL_F7D3302:
 494         case MODEL_F7D4301:
 495         case MODEL_F7D4302:
 496         case MODEL_F5D8235v3:
 497                 if (which == LED_DIAG) {
 498                         // power led gpio: 10 - green, 11 - red
 499                         b = (mode) ? 11 : -10;
 500                         c = (mode) ? -10 : 11;
 501                 } else
 502                         b = f7d[which];
 503                 break;
 504         case MODEL_E1000v2:
 505                 b = e1000v2[which];
 506                 break;
 507         case MODEL_E900:
 508         case MODEL_E1500:
 509         case MODEL_E1550:
 510         case MODEL_E2500:
 511                 b = e900[which];
 512                 break;
 513         case MODEL_E3200:
 514                 b = e3200[which];
 515                 break;
 516         case MODEL_WRT160Nv3:
 517                 b = wrt160nv3[which];
 518                 break;
 519         case MODEL_WRT320N:
 520                 b = wrt320n[which];
 521                 break;
 522         case MODEL_WRT610Nv2:
 523                 b = wrt610nv2[which];
 524                 break;
 525         case MODEL_E4200:
 526                 b = e4200[which];
 527                 break;
 528         case MODEL_RTN10U:
 529                 b = rtn10u[which];
 530                 break;
 531         case MODEL_RTN10P:
 532                 b = rtn10p[which];
 533                 break;
 534         case MODEL_RTN12B1:
 535                 b = rtn12b1[which];
 536                 break;
 537         case MODEL_L600N:
 538                 b = l600n[which];
 539                 break;
 540         case MODEL_DIR620C1:
 541                 b = dir620c1[which];
 542         case MODEL_TDN60:
 543                 b = tdn60[which];
 544         case MODEL_TDN6:
 545                 b = tdn6[which];
 546                 break;
 547 #endif
 548 /*
 549         case MODEL_RT390W:
 550                 break;
 551 */
 552         case MODEL_MN700:
 553                 if (which != LED_DIAG) return ret;
 554                 b = 6;
 555                 break;
 556         case MODEL_WLA2G54L:
 557                 if (which != LED_DIAG) return ret;
 558                 b = 1;
 559                 break;
 560         case MODEL_WRT300N:
 561                 if (which != LED_DIAG) return ret;
 562                 b = 1;
 563                 break;
 564         case MODEL_WRT310Nv1:
 565                 b = wrt310nv1[which];
 566                 break;
 567         case MODEL_WRT160Nv1:
 568                 b = wrt160nv1[which];
 569                 break;
 570         default:
 571                 sprintf(s, "led_%s", led_names[which]);
 572                 if (nvget_gpio(s, &b, &n)) {
 573                         if ((mode != LED_PROBE) && (n)) mode = !mode;
 574                         ret = (n) ? b : ((b) ? -b : -99);
 575                         goto SET;
 576                 }
 577                 return ret;
 578         }
 579
 580         ret = b;
 581         if (b < 0) {
 582                 if (b == -99) b = 0; // -0 substitute
 583                         else b = -b;
 584         }
 585         else if (mode != LED_PROBE) {
 586                 mode = !mode;
 587         }
 588
 589 SET:
 590         if (b < 16) {
 591                 if (mode != LED_PROBE) {
 592                         gpio_write(1 << b, mode);
 593
 594                         if (c < 0) {
 595                                 if (c == -99) c = 0;
 596                                 else c = -c;
 597                         }
 598                         else mode = !mode;
 599                         if (c < 16) gpio_write(1 << c, mode);
 600                 }
 601         }
 602
 603         return ret;
 604 }