Erlang/OTP Forums

typedef struct {
  ErlDrvPort port;
  brd_data_handle_t board;
  int polling;
  int poll_timeout;
} LidData;

static void ready_async(ErlDrvData data, ErlDrvThreadData td)
{

}

static void do_dev_poll(void* data)
{
  LidData *dd = (LidData*)data;
  DrvPollData poll_data;
  int retval;
  ErlDrvBinary *bin;
  uint bin_size = 0;
  const uint hdr_size = 1;
  char hdr[hdr_size+1];
  dbg("do_dev_poll called from thread %0X\r\n",
      pthread_self());
   
  while(dd->polling){
    dbg("polling %u", dd->polling);
    switch (dev_poll(dd->board, &poll_data, dd->poll_timeout)){
    case RET_TIMEOUT:
      dbg("poll timeout");
      hdr[0] = DRV_TIMEOUT;
      driver_output_binary(dd->port, hdr, hdr_size, NULL, 0, 0);
      break;
   
    case RET_OK:
      hdr[0] = DRV_DATA;
      bin_size = 4 + poll_data.size[0] + poll_data.size[1];
      bin = driver_alloc_binary(bin_size);
      *(uint32_t*)bin->orig_bytes = poll_data.code;
      //      dbg("%u %u %u; %p %p %p \n", bin_size, poll_data.size[0], poll_data.size[1], bin->orig_bytes, poll_data.buff[0], poll_data.buff[1]);
      memcpy(bin->orig_bytes + 4, poll_data.buff[0], poll_data.size[0]);
      memcpy(bin->orig_bytes + 4 + poll_data.size[0], poll_data.buff[1], poll_data.size[1]);
      dbg("data from board %s (%x)", dd->board->board_id, poll_data.code);
      driver_output_binary(dd->port, hdr, hdr_size, bin, 0, bin_size);
      if (bin_size)
        driver_free_binary(bin);
      break;
     
    case RET_ERROR:
      dbg("polling error");
      dd->polling = 0;
      hdr[0] = DRV_ERROR;
      driver_output_binary(dd->port, hdr, hdr_size, NULL, 0, 0);
      return;
     
    }
  }
}

static void do_free(void* data)
{
  LidData *dd = (LidData*)data;
  dd->polling = 0;
  dbg("do_free called from thread %0X\r\n",
pthread_self());

}

static ErlDrvData start(ErlDrvPort port, char *command)
{
  char
    board_id[20],

  LidData* dd = (LidData*) driver_alloc(sizeof(LidData));
 
/*
some hw init code here
*/
  dd->port = port;
  set_port_control_flags(port, PORT_CONTROL_FLAG_BINARY);
  dd->polling = 0;
  dd->poll_timeout = DRV_DEFAULT_POLL_TIMEOUT;
 
  dbg("started %s", board_id);
  return (ErlDrvData) dd;
}

static void stop(ErlDrvData handle)
{
  LidData* dd = (LidData*) handle;
  dbg("stopping %s", dd->board->board_id);
  hw_data_free(dd->board);
  driver_free(dd);
}

 
static int control(ErlDrvData drv_data, unsigned int command, char *buf,
                   int len, char **rbuf, int rlen)
{

  LidData *dd = (LidData*)drv_data;
  switch (command){
  case DRV_CTL_START_POLL:
    dbg("ctl start poll");
    dd->polling = 1;
    dbg("control called from thread %0X\r\n",
pthread_self());
     
    driver_async(dd->port, &key, do_dev_poll, dd, do_free);
   
    break;

  case DRV_CTL_POLL_TIMEOUT:
    dd->poll_timeout = (uint32_t)*(uint32_t*)buf;
    dbg("poll_timeout set to %u\n", dd->poll_timeout);
    break;
  default:
    dbg("WTF?! unknown command");
  }
  dbg("ctl ok");
  return 0;
}

static ErlDrvEntry d_entry = {
    .init = NULL,                        /* init */
    .start = start,
    .stop = stop,
    .driver_name = "libhwintf_drv",     /* the name of the driver */
    .control = control,
    .ready_async = ready_async,
};

DRIVER_INIT(libhwintf_drv){
  return &d_entry;
}