pigpiod_if.c
Go to the documentation of this file.
00001 /*
00002 This is free and unencumbered software released into the public domain.
00003 
00004 Anyone is free to copy, modify, publish, use, compile, sell, or
00005 distribute this software, either in source code form or as a compiled
00006 binary, for any purpose, commercial or non-commercial, and by any
00007 means.
00008 
00009 In jurisdictions that recognize copyright laws, the author or authors
00010 of this software dedicate any and all copyright interest in the
00011 software to the public domain. We make this dedication for the benefit
00012 of the public at large and to the detriment of our heirs and
00013 successors. We intend this dedication to be an overt act of
00014 relinquishment in perpetuity of all present and future rights to this
00015 software under copyright law.
00016 
00017 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
00018 EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00019 MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00020 IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
00021 OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00022 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00023 OTHER DEALINGS IN THE SOFTWARE.
00024 
00025 For more information, please refer to <http://unlicense.org/>
00026 */
00027 
00028 /* PIGPIOD_IF_VERSION 27 */
00029 
00030 #include <stdio.h>
00031 #include <stdlib.h>
00032 #include <stdint.h>
00033 #include <string.h>
00034 #include <fcntl.h>
00035 #include <unistd.h>
00036 #include <errno.h>
00037 #include <time.h>
00038 #include <netdb.h>
00039 #include <pthread.h>
00040 
00041 #include <sys/types.h>
00042 #include <sys/stat.h>
00043 #include <sys/time.h>
00044 #include <sys/socket.h>
00045 #include <netinet/tcp.h>
00046 #include <sys/select.h>
00047 
00048 #include <arpa/inet.h>
00049 
00050 #include "pigpio.h"
00051 #include "command.h"
00052 
00053 #include "pigpiod_if.h"
00054 
00055 #define PISCOPE_MAX_REPORTS_PER_READ 4096
00056 
00057 #define STACK_SIZE (256*1024)
00058 
00059 typedef void (*CBF_t) ();
00060 
00061 struct callback_s
00062 {
00063 
00064    int id;
00065    int gpio;
00066    int edge;
00067    CBF_t f;
00068    void * user;
00069    int ex;
00070    callback_t *prev;
00071    callback_t *next;
00072 };
00073 
00074 /* GLOBALS ---------------------------------------------------------------- */
00075 
00076 static gpioReport_t gReport[PISCOPE_MAX_REPORTS_PER_READ];
00077 
00078 static int gPigCommand = -1;
00079 static int gPigHandle = -1;
00080 static int gPigNotify = -1;
00081 
00082 static uint32_t gNotifyBits;
00083 static uint32_t gLastLevel;
00084 
00085 callback_t *gCallBackFirst = 0;
00086 callback_t *gCallBackLast = 0;
00087 
00088 static int gPigStarted = 0;
00089 
00090 static pthread_t *pthNotify;
00091 
00092 static pthread_mutex_t command_mutex = PTHREAD_MUTEX_INITIALIZER;
00093 
00094 /* PRIVATE ---------------------------------------------------------------- */
00095 
00096 static int pigpio_command(int fd, int command, int p1, int p2, int rl)
00097 {
00098    cmdCmd_t cmd;
00099 
00100    cmd.cmd = command;
00101    cmd.p1  = p1;
00102    cmd.p2  = p2;
00103    cmd.res = 0;
00104 
00105    pthread_mutex_lock(&command_mutex);
00106 
00107    if (send(fd, &cmd, sizeof(cmd), 0) != sizeof(cmd))
00108    {
00109       pthread_mutex_unlock(&command_mutex);
00110       return pigif_bad_send;
00111    }
00112 
00113    if (recv(fd, &cmd, sizeof(cmd), MSG_WAITALL) != sizeof(cmd))
00114    {
00115       pthread_mutex_unlock(&command_mutex);
00116       return pigif_bad_recv;
00117    }
00118 
00119    if (rl) pthread_mutex_unlock(&command_mutex);
00120 
00121    return cmd.res;
00122 }
00123 
00124 static int pigpio_command_ext
00125    (int fd, int command, int p1, int p2, int p3,
00126     int extents, gpioExtent_t *ext, int rl)
00127 {
00128    int i;
00129    cmdCmd_t cmd;
00130 
00131    cmd.cmd = command;
00132    cmd.p1  = p1;
00133    cmd.p2  = p2;
00134    cmd.p3  = p3;
00135 
00136    pthread_mutex_lock(&command_mutex);
00137 
00138    if (send(fd, &cmd, sizeof(cmd), 0) != sizeof(cmd))
00139    {
00140       pthread_mutex_unlock(&command_mutex);
00141       return pigif_bad_send;
00142    }
00143 
00144    for (i=0; i<extents; i++)
00145    {
00146       if (send(fd, ext[i].ptr, ext[i].size, 0) != ext[i].size)
00147       {
00148          pthread_mutex_unlock(&command_mutex);
00149          return pigif_bad_send;
00150       }
00151    }
00152 
00153    if (recv(fd, &cmd, sizeof(cmd), MSG_WAITALL) != sizeof(cmd))
00154    {
00155       pthread_mutex_unlock(&command_mutex);
00156       return pigif_bad_recv;
00157    }
00158 
00159    if (rl) pthread_mutex_unlock(&command_mutex);
00160 
00161    return cmd.res;
00162 }
00163 
00164 static int pigpioOpenSocket(char *addr, char *port)
00165 {
00166    int sock, err, opt;
00167    struct addrinfo hints, *res, *rp;
00168    const char *addrStr, *portStr;
00169 
00170    if (!addr)
00171    {
00172       addrStr = getenv(PI_ENVADDR);
00173 
00174       if ((!addrStr) || (!strlen(addrStr)))
00175       {
00176          addrStr = PI_DEFAULT_SOCKET_ADDR_STR;
00177       }
00178    }
00179    else addrStr = addr;
00180 
00181    if (!port)
00182    {
00183       portStr = getenv(PI_ENVPORT);
00184 
00185       if ((!portStr) || (!strlen(portStr)))
00186       {
00187          portStr = PI_DEFAULT_SOCKET_PORT_STR;
00188       }
00189    }
00190    else portStr = port;
00191 
00192    memset (&hints, 0, sizeof (hints));
00193 
00194    hints.ai_family   = PF_UNSPEC;
00195    hints.ai_socktype = SOCK_STREAM;
00196    hints.ai_flags   |= AI_CANONNAME;
00197 
00198    err = getaddrinfo (addrStr, portStr, &hints, &res);
00199 
00200    if (err) return pigif_bad_getaddrinfo;
00201 
00202    for (rp=res; rp!=NULL; rp=rp->ai_next)
00203    {
00204       sock = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
00205 
00206       if (sock == -1) continue;
00207 
00208       /* Disable the Nagle algorithm. */
00209       opt = 1;
00210       setsockopt(sock, IPPROTO_TCP, TCP_NODELAY, (char*)&opt, sizeof(int));
00211 
00212       if (connect(sock, rp->ai_addr, rp->ai_addrlen) != -1) break;
00213    }
00214 
00215    freeaddrinfo(res);
00216 
00217    if (rp == NULL) return pigif_bad_connect;
00218 
00219    return sock;
00220 }
00221 
00222 static void dispatch_notification(gpioReport_t *r)
00223 {
00224    callback_t *p;
00225    uint32_t changed;
00226    int l, g;
00227 
00228    /*
00229    printf("s=%d f=%d l=%8X, t=%10u\n",
00230       r->seqno, r->flags, r->level, r->tick);
00231    */
00232 
00233    if (r->flags == 0)
00234    {
00235       changed = (r->level ^ gLastLevel) & gNotifyBits;
00236 
00237       gLastLevel = r->level;
00238 
00239       p = gCallBackFirst;
00240 
00241       while (p)
00242       {
00243          if (changed & (1<<(p->gpio)))
00244          {
00245             if ((r->level) & (1<<(p->gpio))) l = 1; else l = 0;
00246             if ((p->edge) ^ l)
00247             {
00248                if (p->ex) (p->f)(p->gpio, l, r->tick, p->user);
00249                else       (p->f)(p->gpio, l, r->tick);
00250             }
00251          }
00252          p = p->next;
00253       }
00254    }
00255    else
00256    {
00257       g = (r->flags) & 31;
00258 
00259       p = gCallBackFirst;
00260 
00261       while (p)
00262       {
00263          if ((p->gpio) == g)
00264          {
00265             if (p->ex) (p->f)(g, PI_TIMEOUT, r->tick, p->user);
00266             else       (p->f)(g, PI_TIMEOUT, r->tick);
00267          }
00268          p = p->next;
00269       }
00270    }
00271 }
00272 
00273 static void *pthNotifyThread(void *x)
00274 {
00275    static int got = 0;
00276 
00277    int bytes, r;
00278 
00279    while (1)
00280    {
00281       bytes = read(gPigNotify, (char*)&gReport+got, sizeof(gReport)-got);
00282 
00283       if (bytes > 0) got += bytes;
00284       else break;
00285 
00286       r = 0;
00287 
00288       while (got >= sizeof(gpioReport_t))
00289       {
00290          dispatch_notification(&gReport[r]);
00291 
00292          r++;
00293 
00294          got -= sizeof(gpioReport_t);
00295       }
00296 
00297       /* copy any partial report to start of array */
00298       
00299       if (got && r) gReport[0] = gReport[r];
00300    }
00301    return 0;
00302 }
00303 
00304 static void findNotifyBits(void)
00305 {
00306    callback_t *p;
00307    uint32_t bits = 0;
00308 
00309    p = gCallBackFirst;
00310 
00311    while (p)
00312    {
00313       bits |= (1<<(p->gpio));
00314       p = p->next;
00315    }
00316 
00317    if (bits != gNotifyBits)
00318    {
00319       gNotifyBits = bits;
00320       pigpio_command(gPigCommand, PI_CMD_NB, gPigHandle, gNotifyBits, 1);
00321    }
00322 }
00323 
00324 static void _wfe(unsigned user_gpio, unsigned level, uint32_t tick, void *user)
00325 {
00326    *(int *)user = 1;
00327 }
00328 
00329 static int intCallback(unsigned user_gpio, unsigned edge, void *f, void *user, int ex)
00330 {
00331    static int id = 0;
00332    callback_t *p;
00333 
00334    if ((user_gpio >=0) && (user_gpio < 32) && (edge >=0) && (edge <= 2) && f)
00335    {
00336       /* prevent duplicates */
00337 
00338       p = gCallBackFirst;
00339 
00340       while (p)
00341       {
00342          if ((p->gpio == user_gpio) && (p->edge == edge) && (p->f == f))
00343          {
00344             return pigif_duplicate_callback;
00345          }
00346          p = p->next;
00347       }
00348 
00349       p = malloc(sizeof(callback_t));
00350 
00351       if (p)
00352       {
00353          if (!gCallBackFirst) gCallBackFirst = p;
00354 
00355          p->id = id++;
00356          p->gpio = user_gpio;
00357          p->edge = edge;
00358          p->f = f;
00359          p->user = user;
00360          p->ex = ex;
00361          p->next = 0;
00362          p->prev = gCallBackLast;
00363 
00364          if (p->prev) (p->prev)->next = p;
00365          gCallBackLast = p;
00366 
00367          findNotifyBits();
00368 
00369          return p->id;
00370       }
00371 
00372       return pigif_bad_malloc;
00373    }
00374 
00375    return pigif_bad_callback;
00376 }
00377 
00378 /* PUBLIC ----------------------------------------------------------------- */
00379 
00380 double time_time(void)
00381 {
00382    struct timeval tv;
00383    double t;
00384 
00385    gettimeofday(&tv, 0);
00386 
00387    t = (double)tv.tv_sec + ((double)tv.tv_usec / 1E6);
00388 
00389    return t;
00390 }
00391 
00392 void time_sleep(double seconds)
00393 {
00394    struct timespec ts, rem;
00395 
00396    if (seconds > 0.0)
00397    {
00398       ts.tv_sec = seconds;
00399       ts.tv_nsec = (seconds-(double)ts.tv_sec) * 1E9;
00400 
00401       while (clock_nanosleep(CLOCK_REALTIME, 0, &ts, &rem))
00402       {
00403          /* copy remaining time to ts */
00404          ts.tv_sec  = rem.tv_sec;
00405          ts.tv_nsec = rem.tv_nsec;
00406       }
00407    }
00408 }
00409 
00410 char *pigpio_error(int errnum)
00411 {
00412    if (errnum > -1000) return cmdErrStr(errnum);
00413    else
00414    {
00415       switch(errnum)
00416       {
00417          case pigif_bad_send:
00418             return "failed to send to pigpiod";
00419          case pigif_bad_recv:
00420             return "failed to receive from pigpiod";
00421          case pigif_bad_getaddrinfo:
00422             return "failed to find address of pigpiod";
00423          case pigif_bad_connect:
00424             return "failed to connect to pigpiod";
00425          case pigif_bad_socket:
00426             return "failed to create socket";
00427          case pigif_bad_noib:
00428             return "failed to open noib";
00429          case pigif_duplicate_callback:
00430             return "identical callback exists";
00431          case pigif_bad_malloc:
00432             return "failed to malloc";
00433          case pigif_bad_callback:
00434             return "bad callback parameter";
00435          case pigif_notify_failed:
00436             return "failed to create notification thread";
00437          case pigif_callback_not_found:
00438             return "callback not found";
00439          default:
00440             return "unknown error";
00441       }
00442    }
00443 }
00444 
00445 unsigned pigpiod_if_version(void)
00446 {
00447    return PIGPIOD_IF_VERSION;
00448 }
00449 
00450 pthread_t *start_thread(gpioThreadFunc_t thread_func, void *arg)
00451 {
00452    pthread_t *pth;
00453    pthread_attr_t pthAttr;
00454 
00455    pth = malloc(sizeof(pthread_t));
00456 
00457    if (pth)
00458    {
00459       if (pthread_attr_init(&pthAttr))
00460       {
00461          perror("pthread_attr_init failed");
00462          free(pth);
00463          return NULL;
00464       }
00465 
00466       if (pthread_attr_setstacksize(&pthAttr, STACK_SIZE))
00467       {
00468          perror("pthread_attr_setstacksize failed");
00469          free(pth);
00470          return NULL;
00471       }
00472 
00473       if (pthread_create(pth, &pthAttr, thread_func, arg))
00474       {
00475          perror("pthread_create socket failed");
00476          free(pth);
00477          return NULL;
00478       }
00479    }
00480    return pth;
00481 }
00482 
00483 void stop_thread(pthread_t *pth)
00484 {
00485    if (pth)
00486    {
00487       pthread_cancel(*pth);
00488       pthread_join(*pth, NULL);
00489       free(pth);
00490    }
00491 }
00492 
00493 int pigpio_start(char *addrStr, char *portStr)
00494 {
00495    if ((!addrStr) || (strlen(addrStr) == 0))
00496    {
00497       addrStr = "localhost";
00498    }
00499 
00500    if (!gPigStarted)
00501    {
00502       gPigCommand = pigpioOpenSocket(addrStr, portStr);
00503 
00504       if (gPigCommand >= 0)
00505       {
00506          gPigNotify = pigpioOpenSocket(addrStr, portStr);
00507 
00508          if (gPigNotify >= 0)
00509          {
00510             gPigHandle = pigpio_command(gPigNotify, PI_CMD_NOIB, 0, 0, 1);
00511 
00512             if (gPigHandle < 0) return pigif_bad_noib;
00513             else
00514             {
00515                gLastLevel = read_bank_1();
00516 
00517                pthNotify = start_thread(pthNotifyThread, 0);
00518                if (pthNotify)
00519                {
00520                   gPigStarted = 1;
00521                   return 0;
00522                }
00523                else return pigif_notify_failed;
00524             }
00525          }
00526          else return gPigNotify;
00527       }
00528       else return gPigCommand;
00529    }
00530    return 0;
00531 }
00532 
00533 void pigpio_stop(void)
00534 {
00535    gPigStarted = 0;
00536 
00537    if (pthNotify)
00538    {
00539       stop_thread(pthNotify);
00540       pthNotify = 0;
00541    }
00542 
00543    if (gPigNotify >= 0)
00544    {
00545       if (gPigHandle >= 0)
00546       {
00547          pigpio_command(gPigNotify, PI_CMD_NC, gPigHandle, 0, 1);
00548          gPigHandle = -1;
00549       }
00550 
00551       close(gPigNotify);
00552       gPigNotify = -1;
00553    }
00554 
00555    if (gPigCommand >= 0)
00556    {
00557       if (gPigHandle >= 0)
00558       {
00559          pigpio_command(gPigCommand, PI_CMD_NC, gPigHandle, 0, 1);
00560          gPigHandle = -1;
00561       }
00562 
00563       close(gPigCommand);
00564       gPigCommand = -1;
00565    }
00566 }
00567 
00568 int set_mode(unsigned gpio, unsigned mode)
00569    {return pigpio_command(gPigCommand, PI_CMD_MODES, gpio, mode, 1);}
00570 
00571 int get_mode(unsigned gpio)
00572    {return pigpio_command(gPigCommand, PI_CMD_MODEG, gpio, 0, 1);}
00573 
00574 int set_pull_up_down(unsigned gpio, unsigned pud)
00575    {return pigpio_command(gPigCommand, PI_CMD_PUD, gpio, pud, 1);}
00576 
00577 int gpio_read(unsigned gpio)
00578    {return pigpio_command(gPigCommand, PI_CMD_READ, gpio, 0, 1);}
00579 
00580 int gpio_write(unsigned gpio, unsigned level)
00581    {return pigpio_command(gPigCommand, PI_CMD_WRITE, gpio, level, 1);}
00582 
00583 int set_PWM_dutycycle(unsigned user_gpio, unsigned dutycycle)
00584    {return pigpio_command(gPigCommand, PI_CMD_PWM, user_gpio, dutycycle, 1);}
00585 
00586 int get_PWM_dutycycle(unsigned user_gpio)
00587    {return pigpio_command(gPigCommand, PI_CMD_GDC, user_gpio, 0, 1);}
00588 
00589 int set_PWM_range(unsigned user_gpio, unsigned range)
00590    {return pigpio_command(gPigCommand, PI_CMD_PRS, user_gpio, range, 1);}
00591 
00592 int get_PWM_range(unsigned user_gpio)
00593    {return pigpio_command(gPigCommand, PI_CMD_PRG, user_gpio, 0, 1);}
00594 
00595 int get_PWM_real_range(unsigned user_gpio)
00596    {return pigpio_command(gPigCommand, PI_CMD_PRRG, user_gpio, 0, 1);}
00597 
00598 int set_PWM_frequency(unsigned user_gpio, unsigned frequency)
00599    {return pigpio_command(gPigCommand, PI_CMD_PFS, user_gpio, frequency, 1);}
00600 
00601 int get_PWM_frequency(unsigned user_gpio)
00602    {return pigpio_command(gPigCommand, PI_CMD_PFG, user_gpio, 0, 1);}
00603 
00604 int set_servo_pulsewidth(unsigned user_gpio, unsigned pulsewidth)
00605    {return pigpio_command(gPigCommand, PI_CMD_SERVO, user_gpio, pulsewidth, 1);}
00606 
00607 int get_servo_pulsewidth(unsigned user_gpio)
00608    {return pigpio_command(gPigCommand, PI_CMD_GPW, user_gpio, 0, 1);}
00609 
00610 int notify_open(void)
00611    {return pigpio_command(gPigCommand, PI_CMD_NO, 0, 0, 1);}
00612 
00613 int notify_begin(unsigned handle, uint32_t bits)
00614    {return pigpio_command(gPigCommand, PI_CMD_NB, handle, bits, 1);}
00615 
00616 int notify_pause(unsigned handle)
00617    {return pigpio_command(gPigCommand, PI_CMD_NB, handle, 0, 1);}
00618 
00619 int notify_close(unsigned handle)
00620    {return pigpio_command(gPigCommand, PI_CMD_NC, handle, 0, 1);}
00621 
00622 int set_watchdog(unsigned user_gpio, unsigned timeout)
00623    {return pigpio_command(gPigCommand, PI_CMD_WDOG, user_gpio, timeout, 1);}
00624 
00625 uint32_t read_bank_1(void)
00626    {return pigpio_command(gPigCommand, PI_CMD_BR1, 0, 0, 1);}
00627 
00628 uint32_t read_bank_2(void)
00629    {return pigpio_command(gPigCommand, PI_CMD_BR2, 0, 0, 1);}
00630 
00631 int clear_bank_1(uint32_t levels)
00632    {return pigpio_command(gPigCommand, PI_CMD_BC1, levels, 0, 1);}
00633 
00634 int clear_bank_2(uint32_t levels)
00635    {return pigpio_command(gPigCommand, PI_CMD_BC2, levels, 0, 1);}
00636 
00637 int set_bank_1(uint32_t levels)
00638    {return pigpio_command(gPigCommand, PI_CMD_BS1, levels, 0, 1);}
00639 
00640 int set_bank_2(uint32_t levels)
00641    {return pigpio_command(gPigCommand, PI_CMD_BS2, levels, 0, 1);}
00642 
00643 int hardware_clock(unsigned gpio, unsigned frequency)
00644    {return pigpio_command(gPigCommand, PI_CMD_HC, gpio, frequency, 1);}
00645 
00646 int hardware_PWM(unsigned gpio, unsigned frequency, uint32_t dutycycle)
00647 {
00648    gpioExtent_t ext[1];
00649    
00650    /*
00651    p1=gpio
00652    p2=frequency
00653    p3=4
00654    ## extension ##
00655    uint32_t dutycycle
00656    */
00657 
00658    ext[0].size = sizeof(dutycycle);
00659    ext[0].ptr = &dutycycle;
00660 
00661    return pigpio_command_ext(
00662       gPigCommand, PI_CMD_HP, gpio, frequency, sizeof(dutycycle), 1, ext, 1);
00663 }
00664 
00665 uint32_t get_current_tick(void)
00666    {return pigpio_command(gPigCommand, PI_CMD_TICK, 0, 0, 1);}
00667 
00668 uint32_t get_hardware_revision(void)
00669    {return pigpio_command(gPigCommand, PI_CMD_HWVER, 0, 0, 1);}
00670 
00671 uint32_t get_pigpio_version(void)
00672    {return pigpio_command(gPigCommand, PI_CMD_PIGPV, 0, 0, 1);}
00673 
00674 int wave_clear(void)
00675    {return pigpio_command(gPigCommand, PI_CMD_WVCLR, 0, 0, 1);}
00676 
00677 int wave_add_new(void)
00678    {return pigpio_command(gPigCommand, PI_CMD_WVNEW, 0, 0, 1);}
00679 
00680 int wave_add_generic(unsigned numPulses, gpioPulse_t *pulses)
00681 {
00682    gpioExtent_t ext[1];
00683    
00684    /*
00685    p1=0
00686    p2=0
00687    p3=pulses*sizeof(gpioPulse_t)
00688    ## extension ##
00689    gpioPulse_t[] pulses
00690    */
00691 
00692    if (!numPulses) return 0;
00693 
00694    ext[0].size = numPulses * sizeof(gpioPulse_t);
00695    ext[0].ptr = pulses;
00696 
00697    return pigpio_command_ext(
00698       gPigCommand, PI_CMD_WVAG, 0, 0, ext[0].size, 1, ext, 1);
00699 }
00700 
00701 int wave_add_serial(
00702    unsigned user_gpio, unsigned baud, uint32_t databits,
00703    uint32_t stophalfbits, uint32_t offset,  unsigned numChar, char *str)
00704 {
00705    uint8_t buf[12];
00706    gpioExtent_t ext[2];
00707 
00708    /*
00709    p1=user_gpio
00710    p2=baud
00711    p3=len+12
00712    ## extension ##
00713    uint32_t databits
00714    uint32_t stophalfbits
00715    uint32_t offset
00716    char[len] str
00717    */
00718 
00719    if (!numChar) return 0;
00720 
00721    memcpy(buf, &databits, 4);
00722    memcpy(buf+4, &stophalfbits, 4);
00723    memcpy(buf+8, &offset, 4);
00724 
00725    ext[0].size = sizeof(buf);
00726    ext[0].ptr = buf;
00727 
00728    ext[1].size = numChar;
00729    ext[1].ptr = str;
00730 
00731    return pigpio_command_ext(gPigCommand, PI_CMD_WVAS,
00732       user_gpio, baud, numChar+sizeof(buf), 2, ext, 1);
00733 }
00734 
00735 int wave_create(void)
00736    {return pigpio_command(gPigCommand, PI_CMD_WVCRE, 0, 0, 1);}
00737 
00738 int wave_delete(unsigned wave_id)
00739    {return pigpio_command(gPigCommand, PI_CMD_WVDEL, wave_id, 0, 1);}
00740 
00741 int wave_tx_start(void) /* DEPRECATED */
00742    {return pigpio_command(gPigCommand, PI_CMD_WVGO, 0, 0, 1);}
00743 
00744 int wave_tx_repeat(void) /* DEPRECATED */
00745    {return pigpio_command(gPigCommand, PI_CMD_WVGOR, 0, 0, 1);}
00746 
00747 int wave_send_once(unsigned wave_id)
00748    {return pigpio_command(gPigCommand, PI_CMD_WVTX, wave_id, 0, 1);}
00749 
00750 int wave_send_repeat(unsigned wave_id)
00751    {return pigpio_command(gPigCommand, PI_CMD_WVTXR, wave_id, 0, 1);}
00752 
00753 int wave_chain(char *buf, unsigned bufSize)
00754 {
00755    gpioExtent_t ext[1];
00756 
00757    /*
00758    p1=0
00759    p2=0
00760    p3=bufSize
00761    ## extension ##
00762    char buf[bufSize]
00763    */
00764 
00765    ext[0].size = bufSize;
00766    ext[0].ptr = buf;
00767 
00768    return pigpio_command_ext
00769       (gPigCommand, PI_CMD_WVCHA, 0, 0, bufSize, 1, ext, 1);
00770 }
00771 
00772 int wave_tx_busy(void)
00773    {return pigpio_command(gPigCommand, PI_CMD_WVBSY, 0, 0, 1);}
00774 
00775 int wave_tx_stop(void)
00776    {return pigpio_command(gPigCommand, PI_CMD_WVHLT, 0, 0, 1);}
00777 
00778 int wave_get_micros(void)
00779    {return pigpio_command(gPigCommand, PI_CMD_WVSM, 0, 0, 1);}
00780 
00781 int wave_get_high_micros(void)
00782    {return pigpio_command(gPigCommand, PI_CMD_WVSM, 1, 0, 1);}
00783 
00784 int wave_get_max_micros(void)
00785    {return pigpio_command(gPigCommand, PI_CMD_WVSM, 2, 0, 1);}
00786 
00787 int wave_get_pulses(void)
00788    {return pigpio_command(gPigCommand, PI_CMD_WVSP, 0, 0, 1);}
00789 
00790 int wave_get_high_pulses(void)
00791    {return pigpio_command(gPigCommand, PI_CMD_WVSP, 1, 0, 1);}
00792 
00793 int wave_get_max_pulses(void)
00794    {return pigpio_command(gPigCommand, PI_CMD_WVSP, 2, 0, 1);}
00795 
00796 int wave_get_cbs(void)
00797    {return pigpio_command(gPigCommand, PI_CMD_WVSC, 0, 0, 1);}
00798 
00799 int wave_get_high_cbs(void)
00800    {return pigpio_command(gPigCommand, PI_CMD_WVSC, 1, 0, 1);}
00801 
00802 int wave_get_max_cbs(void)
00803    {return pigpio_command(gPigCommand, PI_CMD_WVSC, 2, 0, 1);}
00804 
00805 int gpio_trigger(unsigned user_gpio, unsigned pulseLen, uint32_t level)
00806 {
00807    gpioExtent_t ext[1];
00808    
00809    /*
00810    p1=user_gpio
00811    p2=pulseLen
00812    p3=4
00813    ## extension ##
00814    unsigned level
00815    */
00816 
00817    ext[0].size = sizeof(uint32_t);
00818    ext[0].ptr = &level;
00819 
00820    return pigpio_command_ext(
00821       gPigCommand, PI_CMD_TRIG, user_gpio, pulseLen, 4, 1, ext, 1);
00822 }
00823 
00824 int set_glitch_filter(unsigned user_gpio, unsigned steady)
00825    {return pigpio_command(gPigCommand, PI_CMD_FG, user_gpio, steady, 1);}
00826 
00827 int set_noise_filter(unsigned user_gpio, unsigned steady, unsigned active)
00828 {
00829    gpioExtent_t ext[1];
00830    
00831    /*
00832    p1=user_gpio
00833    p2=steady
00834    p3=4
00835    ## extension ##
00836    unsigned active
00837    */
00838 
00839    ext[0].size = sizeof(uint32_t);
00840    ext[0].ptr = &active;
00841 
00842    return pigpio_command_ext(
00843       gPigCommand, PI_CMD_FN, user_gpio, steady, 4, 1, ext, 1);
00844 }
00845 
00846 int store_script(char *script)
00847 {
00848    unsigned len;
00849    gpioExtent_t ext[1];
00850 
00851    /*
00852    p1=0
00853    p2=0
00854    p3=len
00855    ## extension ##
00856    char[len] script
00857    */
00858 
00859    len = strlen(script);
00860 
00861    if (!len) return 0;
00862 
00863    ext[0].size = len;
00864    ext[0].ptr = script;
00865 
00866    return pigpio_command_ext(gPigCommand, PI_CMD_PROC, 0, 0, len, 1, ext, 1);
00867 }
00868 
00869 int run_script(unsigned script_id, unsigned numPar, uint32_t *param)
00870 {
00871    gpioExtent_t ext[1];
00872 
00873    /*
00874    p1=script id
00875    p2=0
00876    p3=numPar * 4
00877    ## extension ##
00878    uint32_t[numPar] pars
00879    */
00880 
00881    ext[0].size = 4 * numPar;
00882    ext[0].ptr = param;
00883 
00884    return pigpio_command_ext
00885       (gPigCommand, PI_CMD_PROCR, script_id, 0, numPar*4, 1, ext, 1);
00886 }
00887 
00888 int recvMax(void *buf, int bufsize, int sent)
00889 {
00890    uint8_t scratch[4096];
00891    int remaining, fetch, count;
00892 
00893    if (sent < bufsize) count = sent; else count = bufsize;
00894 
00895    if (count) recv(gPigCommand, buf, count, MSG_WAITALL);
00896 
00897    remaining = sent - count;
00898 
00899    while (remaining)
00900    {
00901       fetch = remaining;
00902       if (fetch > sizeof(scratch)) fetch = sizeof(scratch);
00903       recv(gPigCommand, scratch, fetch, MSG_WAITALL);
00904       remaining -= fetch;
00905    }
00906 
00907    return count;
00908 }
00909 
00910 int script_status(unsigned script_id, uint32_t *param)
00911 {
00912    int status;
00913    uint32_t p[PI_MAX_SCRIPT_PARAMS+1]; /* space for script status */
00914 
00915    status = pigpio_command(gPigCommand, PI_CMD_PROCP, script_id, 0, 0);
00916 
00917    if (status > 0)
00918    {
00919       recvMax(p, sizeof(p), status);
00920       status = p[0];
00921       if (param) memcpy(param, p+1, sizeof(p)-4);
00922    }
00923 
00924    pthread_mutex_unlock(&command_mutex);
00925 
00926    return status;
00927 }
00928 
00929 int stop_script(unsigned script_id)
00930    {return pigpio_command(gPigCommand, PI_CMD_PROCS, script_id, 0, 1);}
00931 
00932 int delete_script(unsigned script_id)
00933    {return pigpio_command(gPigCommand, PI_CMD_PROCD, script_id, 0, 1);}
00934 
00935 int bb_serial_read_open(unsigned user_gpio, unsigned baud, uint32_t bbBits)
00936 {
00937    gpioExtent_t ext[1];
00938    
00939    /*
00940    p1=user_gpio
00941    p2=baud
00942    p3=4
00943    ## extension ##
00944    unsigned bbBits
00945    */
00946 
00947    ext[0].size = sizeof(uint32_t);
00948    ext[0].ptr = &bbBits;
00949 
00950    return pigpio_command_ext(
00951       gPigCommand, PI_CMD_SLRO, user_gpio, baud, 4, 1, ext, 1);
00952 }
00953 
00954 int bb_serial_read(unsigned user_gpio, void *buf, size_t bufSize)
00955 {
00956    int bytes;
00957 
00958    bytes = pigpio_command(gPigCommand, PI_CMD_SLR, user_gpio, bufSize, 0);
00959 
00960    if (bytes > 0)
00961    {
00962       bytes = recvMax(buf, bufSize, bytes);
00963    }
00964 
00965    pthread_mutex_unlock(&command_mutex);
00966 
00967    return bytes;
00968 }
00969 
00970 int bb_serial_read_close(unsigned user_gpio)
00971    {return pigpio_command(gPigCommand, PI_CMD_SLRC, user_gpio, 0, 1);}
00972 
00973 int bb_serial_invert(unsigned user_gpio, unsigned invert)
00974    {return pigpio_command(gPigCommand, PI_CMD_SLRI, user_gpio, invert, 1);}
00975 
00976 int i2c_open(unsigned i2c_bus, unsigned i2c_addr, uint32_t i2c_flags)
00977 {
00978    gpioExtent_t ext[1];
00979 
00980    /*
00981    p1=i2c_bus
00982    p2=i2c_addr
00983    p3=4
00984    ## extension ##
00985    uint32_t i2c_flags
00986    */
00987 
00988    ext[0].size = sizeof(uint32_t);
00989    ext[0].ptr = &i2c_flags;
00990 
00991    return pigpio_command_ext
00992       (gPigCommand, PI_CMD_I2CO, i2c_bus, i2c_addr, 4, 1, ext, 1);
00993 }
00994 
00995 int i2c_close(unsigned handle)
00996    {return pigpio_command(gPigCommand, PI_CMD_I2CC, handle, 0, 1);}
00997 
00998 int i2c_write_quick(unsigned handle, unsigned bit)
00999    {return pigpio_command(gPigCommand, PI_CMD_I2CWQ, handle, bit, 1);}
01000 
01001 int i2c_write_byte(unsigned handle, unsigned val)
01002    {return pigpio_command(gPigCommand, PI_CMD_I2CWS, handle, val, 1);}
01003 
01004 int i2c_read_byte(unsigned handle)
01005    {return pigpio_command(gPigCommand, PI_CMD_I2CRS, handle, 0, 1);}
01006 
01007 int i2c_write_byte_data(unsigned handle, unsigned reg, uint32_t val)
01008 {
01009    gpioExtent_t ext[1];
01010 
01011    /*
01012    p1=handle
01013    p2=reg
01014    p3=4
01015    ## extension ##
01016    uint32_t val
01017    */
01018 
01019    ext[0].size = sizeof(uint32_t);
01020    ext[0].ptr = &val;
01021 
01022    return pigpio_command_ext
01023       (gPigCommand, PI_CMD_I2CWB, handle, reg, 4, 1, ext, 1);
01024 }
01025 
01026 int i2c_write_word_data(unsigned handle, unsigned reg, uint32_t val)
01027 {
01028    gpioExtent_t ext[1];
01029 
01030    /*
01031    p1=handle
01032    p2=reg
01033    p3=4
01034    ## extension ##
01035    uint32_t val
01036    */
01037 
01038    ext[0].size = sizeof(uint32_t);
01039    ext[0].ptr = &val;
01040 
01041    return pigpio_command_ext
01042       (gPigCommand, PI_CMD_I2CWW, handle, reg, 4, 1, ext, 1);
01043 }
01044 
01045 int i2c_read_byte_data(unsigned handle, unsigned reg)
01046    {return pigpio_command(gPigCommand, PI_CMD_I2CRB, handle, reg, 1);}
01047 
01048 int i2c_read_word_data(unsigned handle, unsigned reg)
01049    {return pigpio_command(gPigCommand, PI_CMD_I2CRW, handle, reg, 1);}
01050 
01051 int i2c_process_call(unsigned handle, unsigned reg, uint32_t val)
01052 {
01053    gpioExtent_t ext[1];
01054 
01055    /*
01056    p1=handle
01057    p2=reg
01058    p3=4
01059    ## extension ##
01060    uint32_t val
01061    */
01062 
01063    ext[0].size = sizeof(uint32_t);
01064    ext[0].ptr = &val;
01065 
01066    return pigpio_command_ext
01067       (gPigCommand, PI_CMD_I2CPK, handle, reg, 4, 1, ext, 1);
01068 }
01069 
01070 int i2c_write_block_data(
01071    unsigned handle, unsigned reg, char *buf, unsigned count)
01072 {
01073    gpioExtent_t ext[1];
01074 
01075    /*
01076    p1=handle
01077    p2=reg
01078    p3=count
01079    ## extension ##
01080    char buf[count]
01081    */
01082 
01083    ext[0].size = count;
01084    ext[0].ptr = buf;
01085 
01086    return pigpio_command_ext
01087       (gPigCommand, PI_CMD_I2CWK, handle, reg, count, 1, ext, 1);
01088 }
01089 
01090 int i2c_read_block_data(unsigned handle, unsigned reg, char *buf)
01091 {
01092    int bytes;
01093 
01094    bytes = pigpio_command(gPigCommand, PI_CMD_I2CRK, handle, reg, 0);
01095 
01096    if (bytes > 0)
01097    {
01098       bytes = recvMax(buf, 32, bytes);
01099    }
01100 
01101    pthread_mutex_unlock(&command_mutex);
01102 
01103    return bytes;
01104 }
01105 
01106 int i2c_block_process_call(
01107    unsigned handle, unsigned reg, char *buf, unsigned count)
01108 {
01109    int bytes;
01110    gpioExtent_t ext[1];
01111 
01112    /*
01113    p1=handle
01114    p2=reg
01115    p3=count
01116    ## extension ##
01117    char buf[count]
01118    */
01119 
01120    ext[0].size = count;
01121    ext[0].ptr = buf;
01122 
01123    bytes = pigpio_command_ext
01124       (gPigCommand, PI_CMD_I2CPK, handle, reg, count, 1, ext, 0);
01125 
01126    if (bytes > 0)
01127    {
01128       bytes = recvMax(buf, 32, bytes);
01129    }
01130 
01131    pthread_mutex_unlock(&command_mutex);
01132 
01133    return bytes;
01134 }
01135 
01136 int i2c_read_i2c_block_data(
01137    unsigned handle, unsigned reg, char *buf, uint32_t count)
01138 {
01139    int bytes;
01140    gpioExtent_t ext[1];
01141 
01142    /*
01143    p1=handle
01144    p2=reg
01145    p3=4
01146    ## extension ##
01147    uint32_t count
01148    */
01149 
01150    ext[0].size = sizeof(uint32_t);
01151    ext[0].ptr = &count;
01152 
01153    bytes = pigpio_command_ext
01154       (gPigCommand, PI_CMD_I2CRI, handle, reg, 4, 1, ext, 0);
01155 
01156    if (bytes > 0)
01157    {
01158       bytes = recvMax(buf, count, bytes);
01159    }
01160 
01161    pthread_mutex_unlock(&command_mutex);
01162 
01163    return bytes;
01164 }
01165 
01166 
01167 int i2c_write_i2c_block_data(
01168    unsigned handle, unsigned reg, char *buf, unsigned count)
01169 {
01170    gpioExtent_t ext[1];
01171 
01172    /*
01173    p1=handle
01174    p2=reg
01175    p3=count
01176    ## extension ##
01177    char buf[count]
01178    */
01179 
01180    ext[0].size = count;
01181    ext[0].ptr = buf;
01182 
01183    return pigpio_command_ext
01184       (gPigCommand, PI_CMD_I2CWI, handle, reg, count, 1, ext, 1);
01185 }
01186 
01187 int i2c_read_device(unsigned handle, char *buf, unsigned count)
01188 {
01189    int bytes;
01190 
01191    bytes = pigpio_command(gPigCommand, PI_CMD_I2CRD, handle, count, 0);
01192 
01193    if (bytes > 0)
01194    {
01195       bytes = recvMax(buf, count, bytes);
01196    }
01197 
01198    pthread_mutex_unlock(&command_mutex);
01199 
01200    return bytes;
01201 }
01202 
01203 int i2c_write_device(unsigned handle, char *buf, unsigned count)
01204 {
01205    gpioExtent_t ext[1];
01206 
01207    /*
01208    p1=handle
01209    p2=0
01210    p3=count
01211    ## extension ##
01212    char buf[count]
01213    */
01214 
01215    ext[0].size = count;
01216    ext[0].ptr = buf;
01217 
01218    return pigpio_command_ext
01219       (gPigCommand, PI_CMD_I2CWD, handle, 0, count, 1, ext, 1);
01220 }
01221 
01222 int i2c_zip(
01223    unsigned handle,
01224    char    *inBuf,
01225    unsigned inLen,
01226    char    *outBuf,
01227    unsigned outLen)
01228 {
01229    int bytes;
01230    gpioExtent_t ext[1];
01231 
01232    /*
01233    p1=handle
01234    p2=0
01235    p3=inLen
01236    ## extension ##
01237    char inBuf[inLen]
01238    */
01239 
01240    ext[0].size = inLen;
01241    ext[0].ptr = inBuf;
01242 
01243    bytes = pigpio_command_ext
01244       (gPigCommand, PI_CMD_I2CZ, handle, 0, inLen, 1, ext, 0);
01245 
01246    if (bytes > 0)
01247    {
01248       bytes = recvMax(outBuf, outLen, bytes);
01249    }
01250 
01251    pthread_mutex_unlock(&command_mutex);
01252 
01253    return bytes;
01254 }
01255 
01256 int bb_i2c_open(unsigned SDA, unsigned SCL, unsigned baud)
01257 {
01258    gpioExtent_t ext[1];
01259 
01260    /*
01261    p1=SDA
01262    p2=SCL
01263    p3=4
01264    ## extension ##
01265    uint32_t baud
01266    */
01267 
01268    ext[0].size = sizeof(uint32_t);
01269    ext[0].ptr = &baud;
01270 
01271    return pigpio_command_ext
01272       (gPigCommand, PI_CMD_BI2CO, SDA, SCL, 4, 1, ext, 1);
01273 }
01274 
01275 int bb_i2c_close(unsigned SDA)
01276    {return pigpio_command(gPigCommand, PI_CMD_BI2CC, SDA, 0, 1);}
01277 
01278 int bb_i2c_zip(
01279    unsigned SDA,
01280    char    *inBuf,
01281    unsigned inLen,
01282    char    *outBuf,
01283    unsigned outLen)
01284 {
01285    int bytes;
01286    gpioExtent_t ext[1];
01287 
01288    /*
01289    p1=SDA
01290    p2=0
01291    p3=inLen
01292    ## extension ##
01293    char inBuf[inLen]
01294    */
01295 
01296    ext[0].size = inLen;
01297    ext[0].ptr = inBuf;
01298 
01299    bytes = pigpio_command_ext
01300       (gPigCommand, PI_CMD_BI2CZ, SDA, 0, inLen, 1, ext, 0);
01301 
01302    if (bytes > 0)
01303    {
01304       bytes = recvMax(outBuf, outLen, bytes);
01305    }
01306 
01307    pthread_mutex_unlock(&command_mutex);
01308 
01309    return bytes;
01310 }
01311 
01312 int spi_open(unsigned channel, unsigned speed, uint32_t flags)
01313 {
01314    gpioExtent_t ext[1];
01315 
01316    /*
01317    p1=channel
01318    p2=speed
01319    p3=4
01320    ## extension ##
01321    uint32_t flags
01322    */
01323 
01324    ext[0].size = sizeof(uint32_t);
01325    ext[0].ptr = &flags;
01326 
01327    return pigpio_command_ext
01328       (gPigCommand, PI_CMD_SPIO, channel, speed, 4, 1, ext, 1);
01329 }
01330 
01331 int spi_close(unsigned handle)
01332    {return pigpio_command(gPigCommand, PI_CMD_SPIC, handle, 0, 1);}
01333 
01334 int spi_read(unsigned handle, char *buf, unsigned count)
01335 {
01336    int bytes;
01337 
01338    bytes = pigpio_command
01339       (gPigCommand, PI_CMD_SPIR, handle, count, 0);
01340 
01341    if (bytes > 0)
01342    {
01343       bytes = recvMax(buf, count, bytes);
01344    }
01345 
01346    pthread_mutex_unlock(&command_mutex);
01347 
01348    return bytes;
01349 }
01350 
01351 int spi_write(unsigned handle, char *buf, unsigned count)
01352 {
01353    gpioExtent_t ext[1];
01354 
01355    /*
01356    p1=handle
01357    p2=0
01358    p3=count
01359    ## extension ##
01360    char buf[count]
01361    */
01362 
01363    ext[0].size = count;
01364    ext[0].ptr = buf;
01365 
01366    return pigpio_command_ext
01367       (gPigCommand, PI_CMD_SPIW, handle, 0, count, 1, ext, 1);
01368 }
01369 
01370 int spi_xfer(unsigned handle, char *txBuf, char *rxBuf, unsigned count)
01371 {
01372    int bytes;
01373    gpioExtent_t ext[1];
01374 
01375    /*
01376    p1=handle
01377    p2=0
01378    p3=count
01379    ## extension ##
01380    char buf[count]
01381    */
01382 
01383    ext[0].size = count;
01384    ext[0].ptr = txBuf;
01385 
01386    bytes = pigpio_command_ext
01387       (gPigCommand, PI_CMD_SPIX, handle, 0, count, 1, ext, 0);
01388 
01389    if (bytes > 0)
01390    {
01391       bytes = recvMax(rxBuf, count, bytes);
01392    }
01393 
01394    pthread_mutex_unlock(&command_mutex);
01395 
01396    return bytes;
01397 }
01398 
01399 int serial_open(char *dev, unsigned baud, unsigned flags)
01400 {
01401    int len;
01402    gpioExtent_t ext[1];
01403 
01404    len = strlen(dev);
01405 
01406    /*
01407    p1=baud
01408    p2=flags
01409    p3=len
01410    ## extension ##
01411    char dev[len]
01412    */
01413 
01414    ext[0].size = len;
01415    ext[0].ptr = dev;
01416 
01417    return pigpio_command_ext
01418       (gPigCommand, PI_CMD_SERO, baud, flags, len, 1, ext, 1);
01419 }
01420 
01421 int serial_close(unsigned handle)
01422    {return pigpio_command(gPigCommand, PI_CMD_SERC, handle, 0, 1);}
01423 
01424 int serial_write_byte(unsigned handle, unsigned val)
01425    {return pigpio_command(gPigCommand, PI_CMD_SERWB, handle, val, 1);}
01426 
01427 int serial_read_byte(unsigned handle)
01428    {return pigpio_command(gPigCommand, PI_CMD_SERRB, handle, 0, 1);}
01429 
01430 int serial_write(unsigned handle, char *buf, unsigned count)
01431 {
01432    gpioExtent_t ext[1];
01433 
01434    /*
01435    p1=handle
01436    p2=0
01437    p3=count
01438    ## extension ##
01439    char buf[count]
01440    */
01441 
01442    ext[0].size = count;
01443    ext[0].ptr = buf;
01444 
01445    return pigpio_command_ext
01446       (gPigCommand, PI_CMD_SERW, handle, 0, count, 1, ext, 1);
01447 }
01448 
01449 int serial_read(unsigned handle, char *buf, unsigned count)
01450 {
01451    int bytes;
01452 
01453    bytes = pigpio_command
01454       (gPigCommand, PI_CMD_SERR, handle, count, 0);
01455 
01456    if (bytes > 0)
01457    {
01458       bytes = recvMax(buf, count, bytes);
01459    }
01460 
01461    pthread_mutex_unlock(&command_mutex);
01462 
01463    return bytes;
01464 }
01465 
01466 int serial_data_available(unsigned handle)
01467    {return pigpio_command(gPigCommand, PI_CMD_SERDA, handle, 0, 1);}
01468 
01469 int custom_1(unsigned arg1, unsigned arg2, char *argx, unsigned count)
01470 {
01471    gpioExtent_t ext[1];
01472 
01473    /*
01474    p1=arg1
01475    p2=arg2
01476    p3=count
01477    ## extension ##
01478    char argx[count]
01479    */
01480 
01481    ext[0].size = count;
01482    ext[0].ptr = argx;
01483 
01484    return pigpio_command_ext(
01485       gPigCommand, PI_CMD_CF1, arg1, arg2, count, 1, ext, 1);
01486 }
01487 
01488 
01489 int custom_2(unsigned arg1, char *argx, unsigned count,
01490              char *retBuf, uint32_t retMax)
01491 {
01492    int bytes;
01493    gpioExtent_t ext[1];
01494 
01495    /*
01496    p1=arg1
01497    p2=retMax
01498    p3=count
01499    ## extension ##
01500    char argx[count]
01501    */
01502 
01503    ext[0].size = count;
01504    ext[0].ptr = argx;
01505 
01506    bytes = pigpio_command_ext
01507       (gPigCommand, PI_CMD_CF2, arg1, retMax, count, 1, ext, 0);
01508 
01509    if (bytes > 0)
01510    {
01511       bytes = recvMax(retBuf, retMax, bytes);
01512    }
01513 
01514    pthread_mutex_unlock(&command_mutex);
01515 
01516    return bytes;
01517 }
01518 
01519 
01520 int callback(unsigned user_gpio, unsigned edge, CBFunc_t f)
01521    {return intCallback(user_gpio, edge, f, 0, 0);}
01522 
01523 int callback_ex(unsigned user_gpio, unsigned edge, CBFuncEx_t f, void *user)
01524    {return intCallback(user_gpio, edge, f, user, 1);}
01525 
01526 int callback_cancel(unsigned id)
01527 {
01528    callback_t *p;
01529 
01530    p = gCallBackFirst;
01531 
01532    while (p)
01533    {
01534       if (p->id == id)
01535       {
01536          if (p->prev) p->prev->next = p->next;
01537          else gCallBackFirst = p->next;
01538 
01539          if (p->next) p->next->prev = p->prev;
01540          else gCallBackLast = p->prev;
01541 
01542          free(p);
01543 
01544          findNotifyBits();
01545 
01546          return 0;
01547       }
01548       p = p->next;
01549    }
01550    return pigif_callback_not_found;
01551 }
01552 
01553 int wait_for_edge(unsigned user_gpio, unsigned edge, double timeout)
01554 {
01555    int triggered = 0;
01556    int id;
01557    double due;
01558 
01559    if (timeout <= 0.0) return 0;
01560 
01561    due = time_time() + timeout;
01562 
01563    id = callback_ex(user_gpio, edge, _wfe, &triggered);
01564 
01565    while (!triggered && (time_time() < due)) time_sleep(0.1);
01566 
01567    callback_cancel(id);
01568 
01569    return triggered;
01570 }
01571 
01572 


cob_hand_bridge
Author(s): Mathias Lüdtke
autogenerated on Thu Jun 6 2019 20:43:57