git.gag.com Git - debian/gnuradio/blob - gr-comedi/src/comedi_sink_s.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2005 Free Software Foundation, Inc.
   4  *
   5  * This file is part of GNU Radio
   6  *
   7  * GNU Radio is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 3, or (at your option)
  10  * any later version.
  11  *
  12  * GNU Radio is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with GNU Radio; see the file COPYING.  If not, write to
  19  * the Free Software Foundation, Inc., 51 Franklin Street,
  20  * Boston, MA 02110-1301, USA.
  21  */
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 #include <sys/mman.h>
  28
  29 #include <comedi_sink_s.h>
  30 #include <gr_io_signature.h>
  31 #include <stdio.h>
  32 #include <errno.h>
  33 #include <iostream>
  34 #include <stdexcept>
  35 #include <gri_comedi.h>
  36
  37
  38 /*
  39  * comedi_sink_s is untested because I don't own appropriate hardware.
  40  * Feedback is welcome!  --SF
  41  */
  42
  43 static std::string
  44 default_device_name ()
  45 {
  46   return "/dev/comedi0";
  47 }
  48
  49
  50 // ----------------------------------------------------------------
  51
  52 comedi_sink_s_sptr
  53 comedi_make_sink_s (int sampling_freq, const std::string dev)
  54 {
  55   return comedi_sink_s_sptr (new comedi_sink_s (sampling_freq, dev));
  56 }
  57
  58 comedi_sink_s::comedi_sink_s (int sampling_freq,
  59                                   const std::string device_name)
  60   : gr_sync_block ("comedi_sink_s",
  61                    gr_make_io_signature (0, 0, 0),
  62                    gr_make_io_signature (0, 0, 0)),
  63     d_sampling_freq (sampling_freq),
  64     d_device_name (device_name.empty() ? default_device_name() : device_name),
  65     d_dev (0),
  66     d_subdevice (COMEDI_SUBD_AO),
  67     d_n_chan (1),       // number of input channels
  68     d_map (0),
  69     d_buffer_size (0),
  70     d_buf_front (0),
  71     d_buf_back (0)
  72 {
  73   int  aref=AREF_GROUND;
  74   int  range=0;
  75
  76   d_dev = comedi_open(d_device_name.c_str());
  77   if (d_dev == 0){
  78     comedi_perror(d_device_name.c_str());
  79     throw std::runtime_error ("comedi_sink_s");
  80   }
  81
  82   unsigned int chanlist[256];
  83
  84   for(int i=0; i<d_n_chan; i++){
  85     chanlist[i]=CR_PACK(i,range,aref);
  86   }
  87
  88   comedi_cmd cmd;
  89   int ret;
  90
  91   ret = comedi_get_cmd_generic_timed(d_dev,d_subdevice,&cmd,(unsigned int)(1e9/sampling_freq));
  92   if(ret<0)
  93     bail ("comedi_get_cmd_generic_timed", comedi_errno());
  94
  95   // TODO: check period_ns is not to far off sampling_freq
  96
  97   d_buffer_size = comedi_get_buffer_size(d_dev, d_subdevice);
  98   if (d_buffer_size <= 0)
  99     bail ("comedi_get_buffer_size", comedi_errno());
 100
 101   d_map = mmap(NULL,d_buffer_size,PROT_WRITE,MAP_SHARED,comedi_fileno(d_dev),0);
 102   if (d_map == MAP_FAILED)
 103     bail ("mmap", errno);
 104
 105   cmd.chanlist = chanlist;
 106   cmd.chanlist_len = d_n_chan;
 107   cmd.scan_end_arg = d_n_chan;
 108
 109   cmd.stop_src=TRIG_NONE;
 110   cmd.stop_arg=0;
 111
 112   /* comedi_command_test() tests a command to see if the
 113    * trigger sources and arguments are valid for the subdevice.
 114    * If a trigger source is invalid, it will be logically ANDed
 115    * with valid values (trigger sources are actually bitmasks),
 116    * which may or may not result in a valid trigger source.
 117    * If an argument is invalid, it will be adjusted to the
 118    * nearest valid value.  In this way, for many commands, you
 119    * can test it multiple times until it passes.  Typically,
 120    * if you can't get a valid command in two tests, the original
 121    * command wasn't specified very well. */
 122   ret = comedi_command_test(d_dev,&cmd);
 123
 124   if(ret<0)
 125     bail ("comedi_command_test", comedi_errno());
 126
 127   ret = comedi_command_test(d_dev,&cmd);
 128
 129   if(ret<0)
 130     bail ("comedi_command_test", comedi_errno());
 131
 132   /* start the command */
 133   ret = comedi_command(d_dev,&cmd);
 134
 135   if(ret<0)
 136     bail ("comedi_command", comedi_errno());
 137
 138   set_output_multiple (d_n_chan*sizeof(sampl_t));
 139
 140   assert(sizeof(sampl_t) == sizeof(short));
 141   set_output_signature (gr_make_io_signature (1, 1, sizeof (sampl_t)));
 142 }
 143
 144 bool
 145 comedi_sink_s::check_topology (int ninputs, int noutputs)
 146 {
 147   if (ninputs > d_n_chan)
 148     throw std::runtime_error ("comedi_sink_s");
 149
 150   return true;
 151 }
 152
 153 comedi_sink_s::~comedi_sink_s ()
 154 {
 155   if (d_map) {
 156     munmap(d_map, d_buffer_size);
 157     d_map = 0;
 158   }
 159
 160   comedi_close(d_dev);
 161 }
 162
 163 int
 164 comedi_sink_s::work (int noutput_items,
 165                        gr_vector_const_void_star &input_items,
 166                        gr_vector_void_star &output_items)
 167 {
 168   int ret;
 169
 170   int work_left = noutput_items * sizeof(sampl_t) * d_n_chan;
 171   sampl_t *pbuf = (sampl_t*)d_map;
 172
 173   do {
 174
 175     do {
 176       ret = comedi_get_buffer_contents(d_dev,d_subdevice);
 177       if (ret < 0)
 178         bail ("comedi_get_buffer_contents", comedi_errno());
 179
 180       assert(ret % sizeof(sampl_t) == 0);
 181       assert(work_left % sizeof(sampl_t) == 0);
 182
 183       ret = std::min(ret, work_left);
 184       d_buf_front += ret;
 185
 186       assert(d_buffer_size%d_n_chan == 0);
 187       if (d_buf_front-d_buf_back > (unsigned)d_buffer_size) {
 188               d_buf_front+=d_buffer_size;
 189               d_buf_back +=d_buffer_size;
 190       }
 191
 192       if(d_buf_front==d_buf_back){
 193         usleep(1000000*std::min(work_left,d_buffer_size/2)/(d_sampling_freq*sizeof(sampl_t)*d_n_chan));
 194         continue;
 195       }
 196     } while (d_buf_front==d_buf_back);
 197
 198     for(unsigned i=d_buf_back/sizeof(sampl_t);i<d_buf_front/sizeof(sampl_t);i++){
 199       int chan = i%d_n_chan;
 200       int i_idx = noutput_items-work_left/d_n_chan/sizeof(sampl_t)+(i-d_buf_back/sizeof(sampl_t))/d_n_chan;
 201
 202       pbuf[i%(d_buffer_size/sizeof(sampl_t))] = input_items[chan]==0 ? 0 :
 203                         (int)((short*)(input_items[chan]))[i_idx] + 32767;
 204     }
 205
 206     // FIXME: how to tell comedi the buffer is *written* ?
 207     ret = comedi_mark_buffer_read(d_dev,d_subdevice,d_buf_front-d_buf_back);
 208     if(ret<0)
 209       bail ("comedi_mark_buffer_read", comedi_errno());
 210
 211     work_left -= d_buf_front-d_buf_back;
 212
 213     d_buf_back = d_buf_front;
 214
 215   } while(work_left>0);
 216
 217   return noutput_items;
 218 }
 219
 220
 221 void
 222 comedi_sink_s::output_error_msg (const char *msg, int err)
 223 {
 224   fprintf (stderr, "comedi_sink_s[%s]: %s: %s\n",
 225            d_device_name.c_str(), msg,  comedi_strerror(err));
 226 }
 227
 228 void
 229 comedi_sink_s::bail (const char *msg, int err) throw (std::runtime_error)
 230 {
 231   output_error_msg (msg, err);
 232   throw std::runtime_error ("comedi_sink_s");
 233 }