root/trunk/libfreebob/src/libfreebobstreaming/freebob_streaming.c

Revision 237, 89.1 kB (checked in by pieterpalmers, 18 years ago)

- fixed build problem
- removed some compiler warnings

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1 /* $Id$ */
2
3 /*
4  *   FreeBob Streaming API
5  *   FreeBob = Firewire (pro-)audio for linux
6  *
7  *   http://freebob.sf.net
8  *
9  *   Copyright (C) 2005 Pieter Palmers <pieterpalmers@users.sourceforge.net>
10  *
11  *   This program is free software {} you can redistribute it and/or modify
12  *   it under the terms of the GNU General Public License as published by
13  *   the Free Software Foundation {} either version 2 of the License, or
14  *   (at your option) any later version.
15  *
16  *   This program is distributed in the hope that it will be useful,
17  *   but WITHOUT ANY WARRANTY {} without even the implied warranty of
18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  *   GNU General Public License for more details.
20  *
21  *   You should have received a copy of the GNU General Public License
22  *   along with this program {} if not, write to the Free Software
23  *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24  *
25  *
26  *
27  */
28
29 /* freebob_streaming.c
30  *
31  * Implementation of the FreeBob Streaming API
32  *
33  */
34
35 #include "libfreebob/freebob_streaming.h"
36 #include "freebob_streaming_private.h"
37 #include "freebob_connections.h"
38 #include "freebob_debug.h"
39 #include "thread.h"
40 #include "messagebuffer.h"
41
42 #include <signal.h>
43 #include <unistd.h>
44
45 /**
46  * Callbacks
47  */
48
49 static enum raw1394_iso_disposition
50 iso_slave_receive_handler(raw1394handle_t handle, unsigned char *data,
51         unsigned int length, unsigned char channel,
52         unsigned char tag, unsigned char sy, unsigned int cycle,
53         unsigned int dropped);
54        
55 static enum raw1394_iso_disposition
56 iso_master_receive_handler(raw1394handle_t handle, unsigned char *data,
57         unsigned int length, unsigned char channel,
58         unsigned char tag, unsigned char sy, unsigned int cycle,
59         unsigned int dropped);
60        
61 static enum raw1394_iso_disposition
62 iso_slave_transmit_handler(raw1394handle_t handle,
63                 unsigned char *data, unsigned int *length,
64                 unsigned char *tag, unsigned char *sy,
65                 int cycle, unsigned int dropped);
66                
67 static enum raw1394_iso_disposition
68 iso_master_transmit_handler(raw1394handle_t handle,
69                 unsigned char *data, unsigned int *length,
70                 unsigned char *tag, unsigned char *sy,
71                 int cycle, unsigned int dropped);
72                        
73 static int freebob_am824_recv(char *data,
74                                                           int nevents, unsigned int offset, unsigned int dbc,
75                                                           freebob_connection_t *connection);
76
77 static int freebob_am824_xmit(char *data,
78                                                           int nevents, unsigned int offset, unsigned int dbc,
79                                                           freebob_connection_t *connection);
80                                
81
82 int g_verbose=0;
83
84 freebob_device_t *freebob_streaming_init (freebob_device_info_t *device_info, freebob_options_t options) {
85         int i;
86         int c;
87         int err=0;
88         freebob_device_t* dev=NULL;
89         int discover_verbose=0;
90        
91         freebob_messagebuffer_init();
92
93         g_verbose=options.verbose;
94
95         if(g_verbose) discover_verbose=5;
96
97         assert(device_info);
98        
99         printMessage("FreeBob Streaming Device Init\n");
100         printMessage(" Using FreeBob lib version %s\n",freebob_get_version());
101         printMessage(" Device information:\n");
102        
103         printMessage(" Device options:\n");
104         /* driver related setup */
105         printMessage("  Port                     : %d\n",options.port);
106         printMessage("  Device Node Id           : %d\n",options.node_id);
107         printMessage("  Samplerate               : %d\n",options.sample_rate);
108         printMessage("  Period Size              : %d\n",options.period_size);
109         printMessage("  Nb Buffers               : %d\n",options.nb_buffers);
110         printMessage("  Directions               : %X\n",options.directions);
111        
112         // initialize the freebob_device
113         // allocate memory
114         dev=calloc(1,sizeof(freebob_device_t));
115        
116         if(!dev) {
117                 printError("cannot allocate memory for dev structure!\n");
118                 return NULL;
119         }
120        
121         // clear the device structure
122         memset(dev,0,sizeof(freebob_device_t));
123        
124         // copy the arguments to the device structure
125         memcpy(&dev->device_info, device_info, sizeof(freebob_device_info_t));
126         memcpy(&dev->options, &options, sizeof(freebob_options_t));
127        
128         // read in the device specification
129        
130         /* load the connections*/
131         /*
132          * This info should be provided by the Freebob CML application
133          *
134          */
135         freebob_connection_info_t *libfreebob_capture_connections=NULL;
136         freebob_connection_info_t *libfreebob_playback_connections=NULL;
137
138         dev->fb_handle = freebob_new_handle(options.port);
139         if (!dev->fb_handle) {
140                 free(dev);
141                 printError("cannot create libfreebob handle\n");
142                 return NULL;
143         }
144
145         if (freebob_discover_devices(dev->fb_handle, discover_verbose)!=0) {
146                 freebob_destroy_handle(dev->fb_handle);
147                 free(dev);
148                 printError("device discovering failed\n");
149                 return NULL;
150         }
151
152         /* Try and set the samplerate
153          * This should be done after device discovery
154          * but before reading the bus description as the device capabilities can change
155          */
156
157         if(options.node_id > -1) {
158             if (! freebob_set_samplerate(dev->fb_handle, options.node_id, options.sample_rate)) {
159                 freebob_destroy_handle(dev->fb_handle);
160                 free(dev);
161                 printError("Failed to set samplerate...\n");
162                 return NULL;
163             }
164
165         } else {
166             int devices_on_bus = freebob_get_nb_devices_on_bus(dev->fb_handle);
167             debugPrint(DEBUG_LEVEL_STARTUP,"port = %d, devices_on_bus = %d\n", options.port, devices_on_bus);
168                        
169             for(i=0;i<devices_on_bus;i++) {
170                 int node_id=freebob_get_device_node_id(dev->fb_handle, i);
171                 debugPrint(DEBUG_LEVEL_STARTUP,"set samplerate for device = %d, node = %d\n", i, node_id);
172                                
173                 if (! freebob_set_samplerate(dev->fb_handle, node_id, options.sample_rate)) {
174                         freebob_destroy_handle(dev->fb_handle);
175                         free(dev);
176                         printError("Failed to set samplerate...\n");
177                         return NULL;
178                 }
179             }
180
181         }
182
183         /* Read the connection specification
184          */
185  
186         if(!(options.directions & FREEBOB_IGNORE_CAPTURE)) {
187                 libfreebob_capture_connections=freebob_get_connection_info(dev->fb_handle, options.node_id, 0);
188         }
189
190         if(!(options.directions & FREEBOB_IGNORE_PLAYBACK)) {
191                 libfreebob_playback_connections=freebob_get_connection_info(dev->fb_handle, options.node_id, 1);
192         }
193  
194
195         if (libfreebob_capture_connections) {
196                 dev->nb_connections_capture=libfreebob_capture_connections->nb_connections;
197                 // FIXME:
198                 //dev->nb_connections_capture=0;
199         } else {
200                 dev->nb_connections_capture=0;
201         }       
202        
203         if (libfreebob_playback_connections) {
204                 dev->nb_connections_playback=libfreebob_playback_connections->nb_connections;
205                 // FIXME:
206 //              dev->nb_connections_playback=0;
207         } else {
208                 dev->nb_connections_playback=0;
209         }
210         // FIXME: temporary disable the playback
211 //      dev->nb_connections_playback=0;
212        
213         dev->nb_connections=dev->nb_connections_playback+dev->nb_connections_capture;
214         /* see if there are any connections */
215         if (!dev->nb_connections) {
216                 printError("No connections specified, bailing out\n");
217                 if(libfreebob_capture_connections) { free(libfreebob_capture_connections); libfreebob_capture_connections=NULL; }
218                 if(libfreebob_playback_connections) { free(libfreebob_playback_connections); libfreebob_playback_connections=NULL; }
219                 freebob_destroy_handle(dev->fb_handle);
220                 free(dev);
221                
222                 return NULL;
223         }
224        
225         dev->connections=calloc(dev->nb_connections_playback+dev->nb_connections_capture, sizeof(freebob_connection_t));
226        
227        
228         for (c=0;c<dev->nb_connections_capture;c++) {
229                 memcpy(&dev->connections[c].spec, libfreebob_capture_connections->connections[c], sizeof(freebob_connection_spec_t));
230                 dev->connections[c].spec.direction=FREEBOB_CAPTURE;
231         }
232         for (c=0;c<dev->nb_connections_playback;c++) {
233                 memcpy(&dev->connections[c+dev->nb_connections_capture].spec, libfreebob_playback_connections->connections[c], sizeof(freebob_connection_spec_t));
234                 dev->connections[c+dev->nb_connections_capture].spec.direction=FREEBOB_PLAYBACK;
235         }
236        
237         /* Figure out a master connection.
238          * Either it is given in the spec libfreebob
239          * Or it is the first connection defined (capture connections first)
240          */
241         int master_found=FALSE;
242        
243         for (c=0;c<dev->nb_connections_capture+dev->nb_connections_playback;c++) {
244                 if (dev->connections[c].spec.is_master==TRUE) {
245                         master_found=TRUE;
246                         if(dev->options.sample_rate<0) {
247                                 dev->options.sample_rate=dev->connections[c].spec.samplerate;
248                         }
249                         break;
250                 }
251         }
252        
253         if((!master_found) && (dev->nb_connections_capture+dev->nb_connections_playback > 0)) {
254                 dev->connections[0].spec.is_master=TRUE;
255                 if(dev->options.sample_rate<0) {
256                         dev->options.sample_rate=dev->connections[0].spec.samplerate;
257                 }
258         }
259
260         // initialize all connections
261         for(i=0; i < dev->nb_connections; i++) {
262                 freebob_connection_t *connection= &(dev->connections[i]);
263                 if ((err=freebob_streaming_init_connection(dev, connection))<0) {
264                         printError("failed to init connection %d\n",i);
265                         break;
266                 }
267         }
268        
269         if(libfreebob_capture_connections) { freebob_free_connection_info(libfreebob_capture_connections); libfreebob_capture_connections=NULL; }
270         if(libfreebob_playback_connections) { freebob_free_connection_info(libfreebob_playback_connections); libfreebob_playback_connections=NULL; }
271        
272         // prepare the FD map   
273         // the FD map is not stricly nescessary due to dev->fdmap[i]=dev->connections[i];
274         // but makes things flexible at the moment
275        
276         assert(dev->nb_connections==dev->nb_connections_capture+dev->nb_connections_playback);
277        
278         dev->nfds   = dev->nb_connections;
279         dev->pfds   = malloc (sizeof (struct pollfd) * dev->nfds);
280         dev->fdmap  = malloc (sizeof (freebob_connection_t *) * dev->nfds); // holds the connection idx of this fd
281        
282         for(i=0; i < dev->nb_connections; i++) {
283                 freebob_connection_t *connection= &(dev->connections[i]);
284                 dev->fdmap[i]=connection;
285                 dev->pfds[i].fd = raw1394_get_fd (connection->raw_handle);
286                 dev->pfds[i].events = POLLIN;
287                 connection->pfd=&dev->pfds[i];
288         }
289        
290         // check for errors
291         if(err) {
292                 debugPrint(DEBUG_LEVEL_STARTUP, "Cleaning up connections\n");
293                
294                 // the connection that failed doesn't have to be cleaned
295                 i-=1;
296                
297                 for(; i >= 0; i--) {
298                         freebob_connection_t *connection= &(dev->connections[i]);
299                         debugPrint(DEBUG_LEVEL_STARTUP, "Cleaning up connection %d\n",i);
300                         if ((freebob_streaming_cleanup_connection(dev, connection))<0) {
301                                 printError( "Failed to clean connection %d\n",i);
302                         }
303                 }
304                 free(dev->pfds);
305                 free(dev->fdmap);
306                 free(dev->connections);
307                 free(dev);
308                 return NULL;
309         }
310        
311         freebob_destroy_handle(dev->fb_handle);
312        
313         return dev;
314
315 }
316
317 void freebob_streaming_finish(freebob_device_t *dev) {
318         debugPrint(DEBUG_LEVEL_STARTUP,"Cleaning up connections\n");
319         unsigned int i;
320         int err=0;
321        
322         // cleanup all connections
323         for(i=0; i < dev->nb_connections; i++) {
324                 freebob_connection_t *connection= &(dev->connections[i]);
325                 if ((err=freebob_streaming_cleanup_connection(dev, connection))<0) {
326                         printError("Failed to cleanup connection %d\n",i);
327                         return;
328                 }
329         }
330        
331         // free stream structures
332         if(dev->capture_streams) {
333                 free(dev->capture_streams);
334         }
335         if(dev->playback_streams) {
336                 free(dev->playback_streams);
337         }
338         if(dev->synced_capture_streams) {
339                 free(dev->synced_capture_streams);
340         }
341         if(dev->synced_playback_streams) {
342                 free(dev->synced_playback_streams);
343         }
344        
345         // cleanup data structures
346         free(dev->pfds);
347         free(dev->fdmap);
348         free(dev->connections);
349         free(dev);
350        
351         freebob_messagebuffer_exit();
352        
353         return;
354 }
355
356 int freebob_streaming_get_nb_capture_streams(freebob_device_t *dev) {
357         return dev->nb_capture_streams;
358 }
359
360 int freebob_streaming_get_nb_playback_streams(freebob_device_t *dev) {
361         return dev->nb_playback_streams;
362 }
363
364 int freebob_streaming_get_capture_stream_name(freebob_device_t *dev, int i, char* buffer, size_t buffersize) {
365         freebob_stream_t *stream;
366         if(i<dev->nb_capture_streams) {
367                 stream=*(dev->capture_streams+i);
368 /*              return snprintf (buffer, buffersize, "%s_%d_%d_%d_%s",
369                                  "cap", (int) stream->parent->spec.port,
370                                  stream->parent->spec.node & 0x3F,
371                                  stream->parent->spec.plug, stream->spec.name);
372 */
373                 return snprintf (buffer, buffersize, "dev%d%s_%s",
374                                  stream->parent->spec.id , "c", stream->spec.name);
375         } else {
376                 return -1;
377         }
378 }
379
380 int freebob_streaming_get_playback_stream_name(freebob_device_t *dev, int i, char* buffer, size_t buffersize) {
381         freebob_stream_t *stream;
382         if(i<dev->nb_playback_streams) {
383                 stream=*(dev->playback_streams+i);
384 /*              return snprintf (buffer, buffersize, "%s_%d_%d_%d_%s",
385                                  "pbk", (int) stream->parent->spec.port,
386                                  stream->parent->spec.node & 0x3F,
387                                  stream->parent->spec.plug, stream->spec.name);
388 */
389                 return snprintf (buffer, buffersize, "dev%d%s_%s",
390                                  stream->parent->spec.id, "p", stream->spec.name);
391         } else {
392                 return -1;
393         }
394 }
395
396 freebob_streaming_stream_type freebob_streaming_get_capture_stream_type(freebob_device_t *dev, int i) {
397         freebob_stream_t *stream;
398         if(i<dev->nb_capture_streams) {
399                 stream=*(dev->capture_streams+i);
400                 switch (stream->spec.format) {
401                 case IEC61883_STREAM_TYPE_MBLA:
402                         return freebob_stream_type_audio;
403                 case IEC61883_STREAM_TYPE_MIDI:
404                         return freebob_stream_type_midi;
405                 case IEC61883_STREAM_TYPE_SPDIF:
406                 default:
407                         return freebob_stream_type_unknown;
408                 }
409         } else {
410                 return freebob_stream_type_invalid;
411         }
412 }
413
414 freebob_streaming_stream_type freebob_streaming_get_playback_stream_type(freebob_device_t *dev, int i) {
415         freebob_stream_t *stream;
416         if(i<dev->nb_playback_streams) {
417                 stream=*(dev->playback_streams+i);
418                 switch (stream->spec.format) {
419                 case IEC61883_STREAM_TYPE_MBLA:
420                         return freebob_stream_type_audio;
421                 case IEC61883_STREAM_TYPE_MIDI:
422                         return freebob_stream_type_midi;
423                 case IEC61883_STREAM_TYPE_SPDIF:
424                 default:
425                         return freebob_stream_type_unknown;
426                 }
427         } else {
428                 return freebob_stream_type_invalid;
429         }
430 }
431
432 int freebob_streaming_set_stream_buffer(freebob_device_t *dev,  freebob_stream_t *dst, char *b, freebob_streaming_buffer_type t) {
433         assert(dst);
434        
435         // free the preallocated buffer first
436         freebob_streaming_free_stream_buffer(dev,dst);
437        
438         switch (t) {
439                 case freebob_buffer_type_per_stream:
440                         dst->user_buffer=calloc(dev->options.period_size,sizeof(freebob_sample_t));
441                         dst->buffer_type=freebob_buffer_type_per_stream;
442                         break;
443                
444                 case freebob_buffer_type_uint24:
445                         if(b) {
446                                 dst->buffer_type=t;
447                                 dst->user_buffer=b;
448                         } else {
449                                 // allocate a default type
450                                 dst->buffer_type=freebob_buffer_type_per_stream;
451                                 dst->user_buffer=calloc(dev->options.period_size,sizeof(freebob_sample_t));
452                                 return -1;
453                         }
454                         break;
455                        
456                 case freebob_buffer_type_float:
457                         if(b) {
458                                 dst->buffer_type=t;
459                                 dst->user_buffer=b;
460                         } else {
461                                 // allocate a default type
462                                 dst->buffer_type=freebob_buffer_type_per_stream;
463                                 dst->user_buffer=calloc(dev->options.period_size,sizeof(freebob_sample_t));
464                                 return -1;
465                         }
466                         break;
467                
468                 default:
469                         // allocate a default type
470                         dst->buffer_type=freebob_buffer_type_per_stream;
471                         dst->user_buffer=calloc(dev->options.period_size,sizeof(freebob_sample_t));
472                         return -1;
473                        
474         }
475         return 0;
476 }
477
478 void freebob_streaming_free_stream_buffer(freebob_device_t* dev, freebob_stream_t *dst)
479 {
480        
481         if((dst->buffer_type==freebob_buffer_type_per_stream) && (dst->user_buffer)) {
482                 free(dst->user_buffer);
483                 dst->user_buffer=NULL;
484         }
485 }
486
487
488 int freebob_streaming_set_capture_stream_buffer(freebob_device_t *dev, int i, char *buff,  freebob_streaming_buffer_type t) {
489        
490         freebob_stream_t *stream;
491         if(i<dev->nb_capture_streams) {
492                 stream=*(dev->capture_streams+i);
493                 assert(stream);
494                 return freebob_streaming_set_stream_buffer(dev,  stream, buff, t);
495                
496         } else {
497                 return -1;
498         }
499        
500 }
501
502 int freebob_streaming_set_playback_stream_buffer(freebob_device_t *dev, int i, char *buff,  freebob_streaming_buffer_type t) {
503         freebob_stream_t *stream;
504         if(i<dev->nb_playback_streams) {
505                
506                 stream=*(dev->playback_streams+i);
507                
508                 assert(stream);
509                 return freebob_streaming_set_stream_buffer(dev, stream, buff, t);
510                
511         } else {
512                 return -1;
513         }
514 }
515
516 int freebob_streaming_start_thread(freebob_device_t *dev) {
517         int err;
518         // start the packetizer thread
519         // init the packetizer thread communication semaphores
520         if((err=sem_init(&dev->packetizer.transfer_boundary, 0, 0))) {
521                 printError( "Cannot init packet transfer semaphore\n");
522                 return err;
523         } else {
524                 debugPrint(DEBUG_LEVEL_STARTUP,"FREEBOB: successfull init of packet transfer semaphore\n");
525         }
526        
527         dev->packetizer.priority=dev->options.packetizer_priority;
528         dev->packetizer.realtime=dev->options.realtime;
529        
530         // start watchdog
531         if (dev->packetizer.realtime) {
532                 freebob_streaming_start_watchdog(dev);
533         }
534        
535         // start packetizer thread
536         dev->packetizer.run=1;
537        
538         if (freebob_streaming_create_thread(dev, &dev->packetizer.transfer_thread, dev->packetizer.priority, dev->packetizer.realtime, freebob_iso_packet_iterator, (void *)dev)) {
539                 printError("cannot create packet transfer thread");
540                 return -1;
541         } else {
542                 debugPrint(DEBUG_LEVEL_STARTUP,"Created packet transfer thread\n");
543         }
544         return 0;
545 }
546
547 int freebob_streaming_stop_thread(freebob_device_t *dev) {
548         void *status;
549        
550         debugPrint(DEBUG_LEVEL_STARTUP," Stopping packetizer thread...\n");
551         dev->packetizer.run=0;
552        
553        
554         //sem_post(&dev->packetizer.transfer_ack);
555        
556         pthread_join(dev->packetizer.transfer_thread, &status);
557        
558         // stop watchdog
559         if (dev->packetizer.realtime) {
560                 freebob_streaming_stop_watchdog(dev);
561         }
562        
563         // cleanup semaphores
564         sem_destroy(&dev->packetizer.transfer_boundary);
565
566         return 0;
567 }
568
569
570 int freebob_streaming_start(freebob_device_t *dev) {
571         int err;
572         int i;
573        
574         for(i=0; i < dev->nb_connections; i++) {
575                 err=0;
576                 freebob_connection_t *connection= &(dev->connections[i]);
577
578                 int fdf, syt_interval;
579
580                 int samplerate=dev->options.sample_rate;
581
582                 switch (samplerate) {
583                 case 32000:
584                         syt_interval = 8;
585                         fdf = IEC61883_FDF_SFC_32KHZ;
586                         break;
587                 case 44100:
588                         syt_interval = 8;
589                         fdf = IEC61883_FDF_SFC_44K1HZ;
590                         break;
591                 default:
592                 case 48000:
593                         syt_interval = 8;
594                         fdf = IEC61883_FDF_SFC_48KHZ;
595                         break;
596                 case 88200:
597                         syt_interval = 16;
598                         fdf = IEC61883_FDF_SFC_88K2HZ;
599                         break;
600                 case 96000:
601                         syt_interval = 16;
602                         fdf = IEC61883_FDF_SFC_96KHZ;
603                         break;
604                 case 176400:
605                         syt_interval = 32;
606                         fdf = IEC61883_FDF_SFC_176K4HZ;
607                         break;
608                 case 192000:
609                         syt_interval = 32;
610                         fdf = IEC61883_FDF_SFC_192KHZ;
611                         break;
612                 }
613                
614                 if(dev->options.period_size < syt_interval) {
615                         printError("Period size (%d) too small! Samplerate %d requires period >= %d\n",
616                                    dev->options.period_size, samplerate, syt_interval);
617                         return -1;
618                 }
619                 connection->iso.packets_per_period = dev->options.period_size/syt_interval;
620
621                 //connection->plug=0;
622                 connection->iso.hostplug=-1;
623                
624                 // create the CCM PTP connection
625                 connection->iso.do_disconnect=0;
626
627                 // start receiving
628                 switch (connection->spec.direction) {
629                 case FREEBOB_CAPTURE:
630                
631                         debugPrint(DEBUG_LEVEL_STARTUP," creating capture connections...\n");
632                         if (connection->spec.iso_channel < 0) {
633                                 // we need to make the connection ourself
634                                 debugPrint(DEBUG_LEVEL_STARTUP, "Executing CMP procedure...\n");
635                                
636                                 connection->iso.iso_channel = iec61883_cmp_connect(
637                                         connection->raw_handle,
638                                         connection->spec.node | 0xffc0,
639                                         &connection->spec.plug,
640                                         raw1394_get_local_id (connection->raw_handle),
641                                         &connection->iso.hostplug,
642                                         &connection->iso.bandwidth);
643                                        
644                                 connection->iso.do_disconnect=1;
645                                
646                         } else {
647                                 connection->iso.iso_channel=connection->spec.iso_channel;
648                         }
649                        
650                         if (connection->spec.is_master) { //master connection
651                                 dev->sync_master_connection=connection;
652                                 connection->status.master=NULL;
653                                
654                         }
655
656                         // setup the optimal parameters for the raw1394 ISO buffering
657                         connection->iso.packets_per_period=dev->options.period_size/syt_interval;
658                         // hardware interrupts occur when one DMA block is full, and the size of one DMA
659                         // block = PAGE_SIZE. Setting the max_packet_size makes sure that the HW irq
660                         // occurs at a period boundary (optimal CPU use)
661                         // note: try and use 2 interrupts per period for better latency.
662                         connection->iso.max_packet_size=getpagesize() / connection->iso.packets_per_period * 2;
663                         connection->iso.irq_interval=connection->iso.packets_per_period/2;
664
665                         connection->iso.packet_size=4 * (2 + syt_interval * connection->spec.dimension);
666
667                         if (connection->iso.max_packet_size < connection->iso.packet_size) {
668                                 connection->iso.max_packet_size=connection->iso.packet_size;
669                         }
670
671                         /* the receive buffer size doesn't matter for the latency,
672                            but it has a minimal value in order for libraw to operate correctly (300) */
673                         connection->iso.buffers=400;
674
675                         // this is a hack
676                         if(dev->options.period_size < 128) {
677                                 connection->iso.receive_mode=RAW1394_DMA_PACKET_PER_BUFFER;
678                         } else {
679                                 connection->iso.receive_mode=RAW1394_DMA_BUFFERFILL;
680                         }
681
682                 break;
683                 case FREEBOB_PLAYBACK:
684                         debugPrint(DEBUG_LEVEL_STARTUP," creating playback connections...\n");
685                
686                         if (connection->spec.iso_channel < 0) { // we need to make the connection ourself
687                                 debugPrint(DEBUG_LEVEL_STARTUP, "Executing CMP procedure...\n");
688                                 connection->iso.iso_channel = iec61883_cmp_connect(
689                                 connection->raw_handle,
690                                 raw1394_get_local_id (connection->raw_handle),
691                                 &connection->iso.hostplug,
692                                 connection->spec.node | 0xffc0,
693                                 &connection->spec.plug,
694                                 &connection->iso.bandwidth);
695                                
696                                 connection->iso.do_disconnect=1;
697                         } else {
698                                 connection->iso.iso_channel=connection->spec.iso_channel;
699                         }
700
701                         if (connection->spec.is_master) { // master connection
702                                 dev->sync_master_connection=connection;
703                                 connection->status.master=NULL;
704                         }
705
706                         iec61883_cip_init (
707                                 &connection->status.cip,
708                                 IEC61883_FMT_AMDTP,
709                                 fdf,
710                                 samplerate,
711                                 connection->spec.dimension,
712                                 syt_interval);
713                                
714                         iec61883_cip_set_transmission_mode (
715                                 &connection->status.cip,
716                                 IEC61883_MODE_BLOCKING_EMPTY);
717
718
719                         // setup the optimal parameters for the raw1394 ISO buffering
720                         connection->iso.packets_per_period=dev->options.period_size/syt_interval;
721                         // hardware interrupts occur when one DMA block is full, and the size of one DMA
722                         // block = PAGE_SIZE. Setting the max_packet_size makes sure that the HW irq is
723                         // occurs at a period boundary (optimal CPU use)
724                         // note: try and use 2 interrupts per period for better latency.
725                         connection->iso.max_packet_size=getpagesize() / connection->iso.packets_per_period * 2;
726                         connection->iso.irq_interval=connection->iso.packets_per_period / 2;
727
728                         connection->iso.packet_size=4 * (2 + syt_interval * connection->spec.dimension);
729
730                         if (connection->iso.max_packet_size < connection->iso.packet_size) {
731                                 connection->iso.max_packet_size=connection->iso.packet_size;
732                         }
733
734                         /* the transmit buffer size should be as low as possible for latency.
735                         */
736                         connection->iso.buffers=connection->iso.packets_per_period;
737                         if (connection->iso.buffers<10) connection->iso.buffers=10;
738
739                 break;
740                 }
741                
742                 if(connection->iso.iso_channel<0) {
743                         printError("Could not do CMP for connection %d\n",i);
744                         i-=1;
745                         while(i>=0) {
746                                 connection= &(dev->connections[i]);
747                
748                                 if (connection->iso.do_disconnect) {
749                                         // destroy the CCM PTP connection
750                                         switch (connection->spec.direction) {
751                                         case FREEBOB_CAPTURE:
752                                                 iec61883_cmp_disconnect(
753                                                         connection->raw_handle,
754                                                         connection->spec.node | 0xffc0,
755                                                         connection->spec.plug,
756                                                         raw1394_get_local_id (connection->raw_handle),
757                                                         connection->iso.hostplug,
758                                                         connection->iso.iso_channel,
759                                                         connection->iso.bandwidth);
760                                                
761                                         break;
762                                         case FREEBOB_PLAYBACK:
763                                                 iec61883_cmp_disconnect(
764                                                         connection->raw_handle,
765                                                         raw1394_get_local_id (connection->raw_handle),
766                                                         connection->iso.hostplug,
767                                                         connection->spec.node | 0xffc0,
768                                                         connection->spec.plug,
769                                                         connection->iso.iso_channel,
770                                                         connection->iso.bandwidth);
771                        
772                                         break;
773                                         }
774                                 }
775                                 i--;
776                         }
777                         return -1;
778                 }
779         }
780        
781         /* update the sync master pointer for all connections */
782         debugPrint(DEBUG_LEVEL_STARTUP,"Connection summary:\n");
783         int sync_masters_present=0;
784        
785         for(i=0; i < dev->nb_connections; i++) {
786                 err=0;
787                
788                 freebob_connection_t *connection= &(dev->connections[i]);
789                
790                 if (connection->spec.direction == FREEBOB_CAPTURE) {
791                         debugPrint(DEBUG_LEVEL_STARTUP,"  Capture : from node %02X.%02d to node %02X.%02d on channel %02d  {%p} ",
792                                 connection->spec.node & ~0xffc0, connection->spec.plug,
793                                 raw1394_get_local_id (connection->raw_handle) & ~0xffc0, connection->iso.hostplug,
794                                 connection->iso.iso_channel, connection);
795                 } else if (connection->spec.direction == FREEBOB_PLAYBACK) {
796                        
797                         debugPrint(DEBUG_LEVEL_STARTUP,"  Playback: from node %02X.%02d to node %02X.%02d on channel %02d  {%p} ",
798                                 raw1394_get_local_id (connection->raw_handle) & ~0xffc0, connection->iso.hostplug,
799                                 connection->spec.node & ~0xffc0, connection->spec.plug,
800                                 connection->iso.iso_channel, connection);
801                 }
802                
803                 if (connection->spec.is_master) {
804                         sync_masters_present++;
805                        
806                         debugPrintShort(DEBUG_LEVEL_STARTUP," [MASTER]");       
807                 } else {
808                         assert(dev->sync_master_connection);
809                         connection->status.master=&dev->sync_master_connection->status;
810                        
811                         debugPrintShort(DEBUG_LEVEL_STARTUP," [SLAVE]");
812                 }
813                 debugPrintShort(DEBUG_LEVEL_STARTUP,"\n");
814         }
815        
816         if (sync_masters_present == 0) {
817                 printError("no sync master connection present!\n");
818                 // TODO: cleanup
819                 //freebob_streaming_stop(driver);
820                 return -1;
821         } else if (sync_masters_present > 1) {
822                 printError("too many sync master connections present! (%d)\n",sync_masters_present);
823                 // TODO: cleanup
824                 //freebob_streaming_stop(driver);
825                 return -1;
826         }
827        
828         // put nb_periods*period_size of null frames into the playback buffers
829         if((err=freebob_streaming_prefill_playback_streams(dev))<0) {
830                 printError("Could not prefill playback streams.\n");
831                 return err;
832         }
833                
834         // we should transfer nb_buffers-1 periods of playback from the stream buffers to the event buffer
835         for (i=0;i<dev->options.nb_buffers;i++) {
836                 freebob_streaming_transfer_playback_buffers(dev);
837         }
838
839         freebob_streaming_print_bufferfill(dev);
840                
841         debugPrint(DEBUG_LEVEL_STARTUP,"Armed...\n");
842        
843         freebob_streaming_start_thread(dev);
844
845         return 0;
846
847 }
848
849 int freebob_streaming_stop(freebob_device_t *dev) {
850         unsigned int i;
851        
852
853         freebob_streaming_stop_thread(dev);
854        
855         // stop ISO xmit/receive
856         for(i=0; i < dev->nb_connections; i++) {
857                 freebob_connection_t *connection= &(dev->connections[i]);
858
859                 if (connection->iso.do_disconnect) {
860                         // destroy the CCM PTP connection
861                         switch (connection->spec.direction) {
862                         case FREEBOB_CAPTURE:
863                                 iec61883_cmp_disconnect(
864                                         connection->raw_handle,
865                                         connection->spec.node | 0xffc0,
866                                         connection->spec.plug,
867                                         raw1394_get_local_id (connection->raw_handle),
868                                         connection->iso.hostplug,
869                                         connection->iso.iso_channel,
870                                         connection->iso.bandwidth);
871                                
872                         break;
873                         case FREEBOB_PLAYBACK:
874                                 iec61883_cmp_disconnect(
875                                         connection->raw_handle,
876                                         raw1394_get_local_id (connection->raw_handle),
877                                         connection->iso.hostplug,
878                                         connection->spec.node | 0xffc0,
879                                         connection->spec.plug,
880                                         connection->iso.iso_channel,
881                                         connection->iso.bandwidth);
882        
883                         break;
884                         }
885                 }
886                
887         }
888         return 0;
889 }
890
891 void freebob_streaming_print_bufferfill(freebob_device_t *dev) {
892 #ifdef DEBUG   
893     int i=0;
894         for(i=0; i < dev->nb_connections; i++) {
895                 freebob_connection_t *connection= &(dev->connections[i]);
896                 debugPrint(DEBUG_LEVEL_XRUN_RECOVERY, "%i: %d\n",i,freebob_ringbuffer_read_space(connection->event_buffer));
897         }
898 #endif
899 }
900
901 int freebob_streaming_prefill_playback_streams(freebob_device_t *dev) {
902         int i;
903         int err=0;
904        
905         for(i=0; i < dev->nb_playback_streams; i++) {
906                 freebob_stream_t *stream;
907
908                
909                 stream=*(dev->playback_streams+i);
910                
911                 assert(stream);
912                 err=freebob_streaming_prefill_stream(dev, stream);
913                 if (err) {
914                         printError("Could not prefill stream %d\n",i);
915                         return -1;
916                 }
917         }
918         return 0;
919        
920 }
921
922 int freebob_streaming_reset_playback_streams(freebob_device_t *dev) {
923         int i;
924         int err=0;
925        
926         for(i=0; i < dev->nb_playback_streams; i++) {
927                 freebob_stream_t *stream;
928
929                 stream=*(dev->playback_streams+i);
930                
931                 assert(stream);
932                 err=freebob_streaming_reset_stream(dev, stream);
933                 if (err) {
934                         printError("Could not reset stream %d\n",i);
935                         return -1;
936                 }
937         }
938         return 0;
939        
940 }
941
942 int freebob_streaming_reset(freebob_device_t *dev) {
943         /*
944          * Reset means:
945          * 1) Stopping the packetizer thread
946          * 2) Bringing all buffers & connections into a know state
947          *    - Clear all capture buffers
948          *    - Put nb_periods*period_size of null frames into the playback buffers
949          * 3) Restarting the packetizer thread
950          */
951
952         unsigned long i;
953         int err;
954         assert(dev);
955
956         debugPrint(DEBUG_LEVEL_XRUN_RECOVERY, "Resetting streams...\n");
957        
958         if((err=freebob_streaming_stop_thread(dev))<0) {
959                 printError("Could not stop packetizer thread.\n");
960                 return err;
961         }
962
963         //freebob_streaming_stop_iso(dev);
964
965         // check if the packetizer is stopped
966         assert(!dev->packetizer.run);
967        
968         // reset all connections
969         for(i=0; i < dev->nb_connections; i++) {
970                 freebob_connection_t *connection= &(dev->connections[i]);
971                 freebob_streaming_reset_connection(dev,connection);
972         }
973        
974         // put nb_periods*period_size of null frames into the playback buffers
975         if((err=freebob_streaming_prefill_playback_streams(dev))<0) {
976                 printError("Could not prefill playback streams.\n");
977                 return err;
978         }
979        
980         // we should transfer nb_buffers-1 periods of playback from the stream buffers to the event buffer
981         for (i=0;i<dev->options.nb_buffers;i++) {
982                 freebob_streaming_transfer_playback_buffers(dev);
983         }
984                        
985         // clear the xrun flag
986         dev->xrun_detected=FALSE;
987        
988         //freebob_streaming_start_iso(dev);
989         freebob_streaming_print_bufferfill(dev);
990        
991         if((err=freebob_streaming_start_thread(dev))<0) {
992                 printError("Could not start packetizer thread.\n");
993                 return err;
994         }
995        
996        
997         return 0;
998 }
999
1000 int freebob_streaming_xrun_recovery(freebob_device_t *dev) {
1001         freebob_streaming_reset(dev);
1002         dev->xrun_count++;
1003        
1004         return 0;
1005 }
1006
1007 int freebob_streaming_wait(freebob_device_t *dev) {
1008         int ret;
1009
1010         // Wait for packetizer thread to signal a period completion
1011         sem_wait(&dev->packetizer.transfer_boundary);
1012        
1013         // acknowledge the reception of the period
1014         //sem_post(&dev->packetizer.transfer_ack);
1015        
1016         if(dev->xrun_detected) {
1017                 // notify the driver of the underrun and the delay
1018                 ret = (-1) * freebob_streaming_xrun_recovery(dev);
1019         } else {
1020                 ret=dev->options.period_size;
1021         }
1022        
1023         return ret;
1024 }
1025
1026 int freebob_streaming_transfer_capture_buffers(freebob_device_t *dev) {
1027         int i;
1028         int xrun;
1029         unsigned int offset=0;
1030        
1031         freebob_ringbuffer_data_t vec[2];
1032         // transfer the received events into the stream buffers
1033         for (i=0;i<dev->nb_connections_capture;i++) {
1034                 freebob_connection_t *connection= &(dev->connections[i]); // capture connections are first in the array
1035                 assert(connection);
1036                
1037                 debugPrint(DEBUG_LEVEL_WAIT, "R: > %d\n",freebob_ringbuffer_read_space(connection->event_buffer));
1038                        
1039                         // we received one period of frames on each connection
1040                         // this is period_size*dimension of events
1041
1042                 int events2read=dev->options.period_size*connection->spec.dimension;
1043                 int bytes2read=events2read*sizeof(quadlet_t);
1044                        
1045                         // TODO: implement dbc for midi
1046                 int dbc=0;
1047                        
1048                         /*      read events2read bytes from the ringbuffer
1049                 *  first see if it can be done in one read.
1050                 *  if so, ok.
1051                 *  otherwise read up to a multiple of clusters directly from the buffer
1052                 *  then do the buffer wrap around using ringbuffer_read
1053                 *  then read the remaining data directly from the buffer in a third pass
1054                 *  Make sure that we cannot end up on a non-cluster aligned position!
1055                         */
1056                 int cluster_size=connection->spec.dimension*sizeof(quadlet_t);
1057 //              unsigned int frames2read=events2read/connection->spec.dimension;
1058                        
1059                 while(bytes2read>0) {
1060                         unsigned int framesread=(dev->options.period_size*cluster_size-bytes2read)/cluster_size;
1061                         offset=framesread;
1062 //                      offset=0;
1063                        
1064                         int bytesread=0;
1065                                                
1066                         freebob_ringbuffer_get_read_vector(connection->event_buffer, vec);
1067                                
1068                         if(vec[0].len==0) { // this indicates an empty event buffer
1069                                 printError("Event buffer underrun on capture connection %d\n",i);
1070                                 break;
1071                         }
1072                                
1073                                 /* if we don't take care we will get stuck in an infinite loop
1074                         * because we align to a cluster boundary later
1075                         * the remaining nb of bytes in one read operation can be smaller than one cluster
1076                         * this can happen because the ringbuffer size is always a power of 2
1077                                 */
1078                         if(vec[0].len<cluster_size) {
1079                                         // use the ringbuffer function to read one cluster (handles wrap around)
1080                                 freebob_ringbuffer_read(connection->event_buffer,connection->cluster_buffer,cluster_size);
1081                                        
1082                                 // decode the temporary buffer
1083                                 debugPrint(DEBUG_LEVEL_WAIT, "R: %5d [%5d %5d] %5d %5d\n",bytes2read,vec[0].len,vec[1].len,1, offset);
1084                                        
1085                                 xrun = freebob_am824_recv(connection->cluster_buffer, 1, offset, dbc, connection);
1086                                        
1087                                 if(xrun<0) {
1088                                                 // xrun detected
1089                                         printError("Frame buffer overrun on capture connection %d\n",i);
1090                                         break;
1091                                 }
1092                                        
1093                                         // we advanced one cluster_size
1094                                 bytes2read-=cluster_size;
1095                                        
1096                         } else { //
1097                                
1098                                 if(bytes2read>vec[0].len) {
1099                                                 // align to a cluster boundary
1100                                         bytesread=vec[0].len-(vec[0].len%cluster_size);
1101                                 } else {
1102                                         bytesread=bytes2read;
1103                                 }
1104                                        
1105                                 debugPrint(DEBUG_LEVEL_WAIT, "R: %5d [%5d %5d] %5d %5d\n",bytes2read,vec[0].len,vec[1].len,bytesread, offset);
1106                                        
1107                                 xrun = freebob_am824_recv(vec[0].buf, bytesread/sizeof(quadlet_t)/connection->spec.dimension, offset, dbc, connection);
1108                                        
1109                                 if(xrun<0) {
1110                                                 // xrun detected
1111                                         printError("Frame buffer overrun on capture connection %d\n",i);
1112                                         break;
1113                                 }
1114        
1115                                 freebob_ringbuffer_read_advance(connection->event_buffer, bytesread);
1116                                 bytes2read -= bytesread;
1117                         }
1118                                
1119                                 // the bytes2read should always be cluster aligned
1120                         assert(bytes2read%cluster_size==0);
1121                         debugPrint(DEBUG_LEVEL_WAIT, "R: < %d\n",freebob_ringbuffer_read_space(connection->event_buffer));
1122                                
1123                 }
1124
1125         }
1126         return 0;
1127 }
1128
1129 int freebob_streaming_transfer_playback_buffers(freebob_device_t *dev) {
1130         int i;
1131         int xrun;
1132         unsigned int offset=0;
1133        
1134         freebob_ringbuffer_data_t vec[2];
1135         // transfer the output stream buffers content to the event buffers
1136         for (i=dev->nb_connections_capture;i<dev->nb_connections_capture+dev->nb_connections_playback;i++) {
1137                 freebob_connection_t *connection= &(dev->connections[i]);
1138                 assert(connection);
1139                 debugPrint(DEBUG_LEVEL_WAIT, "W: < %d\n",freebob_ringbuffer_write_space(connection->event_buffer));
1140                        
1141                         // we received one period of frames on each connection
1142                         // this is period_size*dimension of events
1143
1144                 int events2write=dev->options.period_size*connection->spec.dimension;
1145                 int bytes2write=events2write*sizeof(quadlet_t);
1146                        
1147                         // TODO: implement dbc for midi
1148                 int dbc=0;
1149                        
1150                 /*      write events2write bytes to the ringbuffer
1151                 *  first see if it can be done in one read.
1152                 *  if so, ok.
1153                 *  otherwise write up to a multiple of clusters directly to the buffer
1154                 *  then do the buffer wrap around using ringbuffer_write
1155                 *  then write the remaining data directly to the buffer in a third pass
1156                 *  Make sure that we cannot end up on a non-cluster aligned position!
1157                 */
1158                 int cluster_size=connection->spec.dimension*sizeof(quadlet_t);
1159 //              unsigned int frames2write=events2write/connection->spec.dimension;
1160                        
1161                 while(bytes2write>0) {
1162                         int byteswritten=0;
1163                        
1164                         unsigned int frameswritten=(dev->options.period_size*cluster_size-bytes2write)/cluster_size;
1165                         offset=frameswritten;
1166 //                      offset=0;
1167                        
1168                         freebob_ringbuffer_get_write_vector(connection->event_buffer, vec);
1169                                
1170                         if(vec[0].len==0) { // this indicates a full event buffer
1171                                 printError("Event buffer overrun on playback connection %d\n",i);
1172                                 break;
1173                         }
1174                                
1175                         /* if we don't take care we will get stuck in an infinite loop
1176                         * because we align to a cluster boundary later
1177                         * the remaining nb of bytes in one write operation can be smaller than one cluster
1178                         * this can happen because the ringbuffer size is always a power of 2
1179                         */
1180                         if(vec[0].len<cluster_size) {
1181                                
1182                                 // encode to the temporary buffer
1183                                 debugPrint(DEBUG_LEVEL_WAIT, "W: %d [%d %d] %d\n",bytes2write,vec[0].len,vec[1].len,1);
1184                                        
1185                                 xrun = freebob_am824_xmit(connection->cluster_buffer, 1, offset, dbc, connection);
1186                                
1187                                 if(xrun<0) {
1188                                                 // xrun detected
1189                                         printError("Frame buffer underrun on playback connection %d\n",i);
1190                                         break;
1191                                 }
1192                                        
1193                                 // use the ringbuffer function to write one cluster (handles wrap around)
1194                                 freebob_ringbuffer_write(connection->event_buffer,connection->cluster_buffer,cluster_size);
1195                                        
1196                                 // we advanced one cluster_size
1197                                 bytes2write-=cluster_size;
1198                                        
1199                         } else { //
1200                                
1201                                 if(bytes2write>vec[0].len) {
1202                                         // align to a cluster boundary
1203                                         byteswritten=vec[0].len-(vec[0].len%cluster_size);
1204                                 } else {
1205                                         byteswritten=bytes2write;
1206                                 }
1207                                        
1208                                 debugPrint(DEBUG_LEVEL_WAIT, "W: %d [%d %d] %d\n",bytes2write,vec[0].len,vec[1].len,byteswritten);
1209                                        
1210                                 xrun = freebob_am824_xmit(vec[0].buf, byteswritten/sizeof(quadlet_t)/connection->spec.dimension, offset, dbc, connection);
1211                                        
1212                                 if(xrun<0) {
1213                                                 // xrun detected
1214                                         printError("Frame buffer underrun on playback connection %d\n",i);
1215                                         break;
1216                                 }
1217        
1218                                 freebob_ringbuffer_write_advance(connection->event_buffer, byteswritten);
1219                                 bytes2write -= byteswritten;
1220                         }
1221                                
1222                         // the bytes2write should always be cluster aligned
1223                         assert(bytes2write%cluster_size==0);
1224                                
1225                 }
1226
1227                 debugPrint(DEBUG_LEVEL_WAIT, "W: > %d\n",freebob_ringbuffer_write_space(connection->event_buffer));
1228         }
1229        
1230        
1231         return 0;
1232 }
1233
1234 int freebob_streaming_transfer_buffers(freebob_device_t *dev) {
1235         int err=0;
1236        
1237         err=freebob_streaming_transfer_capture_buffers(dev);
1238         if (err) return err;
1239        
1240         err=freebob_streaming_transfer_playback_buffers(dev);
1241         return err;
1242        
1243 }
1244
1245
1246 int freebob_streaming_write(freebob_device_t *dev, int i, freebob_sample_t *buffer, int nsamples) {
1247         int retval;
1248        
1249         freebob_stream_t *stream;
1250         assert(i<dev->nb_playback_streams);
1251        
1252         stream=*(dev->playback_streams+i);
1253         assert(stream);
1254        
1255         retval=freebob_ringbuffer_write(stream->buffer,(char *)buffer,nsamples*sizeof(freebob_sample_t))/sizeof(freebob_sample_t);
1256                
1257         return retval;
1258 }
1259
1260 int freebob_streaming_read(freebob_device_t *dev, int i, freebob_sample_t *buffer, int nsamples) {
1261         // this has one ringbuffer too many, but that will be solved shortly
1262         int retval=0;
1263
1264         freebob_stream_t *stream;
1265         assert(i<dev->nb_capture_streams);
1266        
1267         stream=*(dev->capture_streams+i);
1268         assert(stream);
1269        
1270         // now read from the stream ringbuffer
1271         retval=freebob_ringbuffer_read(stream->buffer, (char *)buffer, nsamples*sizeof(freebob_sample_t))/sizeof(freebob_sample_t);     
1272
1273 //      fprintf(stderr,"rb read1 [%02d: %08p %08p %08X, %d, %d]\n",stream->spec.position, stream, stream->buffer, *buffer, nsamples, retval);
1274        
1275         return retval;
1276 }
1277
1278 pthread_t freebob_streaming_get_packetizer_thread(freebob_device_t *dev) {
1279         return dev->packetizer.transfer_thread;
1280 }
1281
1282 /* --------------------- *
1283  * PRIVATE STUFF         *
1284  * --------------------- */
1285  
1286  
1287 unsigned int freebob_streaming_register_generic_stream(freebob_stream_t *stream, freebob_stream_t ***oldset, unsigned int set_size) {
1288         int i;
1289         int found=0;
1290         unsigned int new_set_size;
1291        
1292         freebob_stream_t **new_streams=calloc(set_size+1,sizeof(freebob_stream_t *));
1293         freebob_stream_t **set=*oldset;
1294        
1295         for (i=0;i<set_size;i++) {
1296                 *(new_streams+i)=*(set+i);
1297                 if(*(set+i)==stream) {
1298                         printError("stream already registered\n");
1299                         found=1;
1300                 }
1301         }
1302         if (!found) {
1303                 *(new_streams+set_size)=stream;
1304                 new_set_size=set_size+1;
1305                
1306                 free(*oldset);
1307                 *oldset=new_streams;
1308         } else {
1309                 free(new_streams);
1310                 new_set_size=set_size;
1311         }
1312         return new_set_size;
1313        
1314 }
1315
1316 void freebob_streaming_register_capture_stream(freebob_device_t *dev, freebob_stream_t *stream) {
1317         dev->nb_capture_streams=
1318                 freebob_streaming_register_generic_stream(
1319                         stream,
1320                         &dev->capture_streams,
1321                         dev->nb_capture_streams);
1322         if (stream->spec.format==IEC61883_STREAM_TYPE_MBLA) {
1323                 dev->nb_synced_capture_streams=
1324                         freebob_streaming_register_generic_stream(
1325                                 stream,
1326                                 &dev->synced_capture_streams,
1327                                 dev->nb_synced_capture_streams);
1328         }
1329 }
1330
1331 void freebob_streaming_register_playback_stream(freebob_device_t *dev, freebob_stream_t *stream) {
1332         dev->nb_playback_streams=
1333                 freebob_streaming_register_generic_stream(
1334                         stream,
1335                         &dev->playback_streams,
1336                         dev->nb_playback_streams);
1337         if (stream->spec.format==IEC61883_STREAM_TYPE_MBLA) {
1338                 dev->nb_synced_playback_streams=
1339                         freebob_streaming_register_generic_stream(
1340                                 stream,
1341                                 &dev->synced_playback_streams,
1342                                 dev->nb_synced_playback_streams);
1343         }
1344 }
1345
1346 static inline int
1347 freebob_streaming_period_complete (freebob_device_t *dev) {
1348         unsigned long i;
1349         //unsigned int period_boundary_bytes=dev->options.period_size*sizeof(freebob_sample_t);
1350        
1351         assert(dev);
1352        
1353         for(i=0; i < dev->nb_connections; i++) {
1354                 freebob_connection_t *connection= &(dev->connections[i]);
1355                 if (connection->status.frames_left > 0) {
1356                         return FALSE;
1357                 }
1358         }
1359         return TRUE;
1360        
1361 #if 0   
1362         for(i=0; i < dev->nb_synced_capture_streams; i++) {
1363                 assert(dev->synced_capture_streams);
1364                 assert(dev->synced_capture_streams[i]);
1365                
1366                 /* We check if there is more than one period of samples in the buffer.
1367                  * if not the period isn't complete yet.
1368                  */
1369                 if (freebob_ringbuffer_read_space(dev->synced_capture_streams[i]->buffer) < period_boundary_bytes) {
1370                         return FALSE;
1371                 }
1372         }
1373        
1374         for(i=0; i < dev->nb_synced_playback_streams; i++) {
1375                 assert(dev->synced_playback_streams);
1376                 assert(dev->synced_playback_streams[i]);
1377                 //FIXME: check this!
1378                 /* We check if there is still one period of samples in the output buffer.
1379                  * if so, the period isn't complete yet.
1380                  */
1381                 if (freebob_ringbuffer_read_space(dev->synced_playback_streams[i]->buffer) > period_boundary_bytes) {
1382                         return FALSE;
1383                 }
1384         }
1385         return TRUE;
1386 #endif
1387 }
1388
1389 static inline void
1390 freebob_streaming_period_reset (freebob_device_t *dev) {
1391 //      unsigned long i;
1392         //unsigned int period_boundary_bytes=dev->options.period_size*sizeof(freebob_sample_t);
1393         unsigned long i;
1394         //unsigned int period_boundary_bytes=dev->options.period_size*sizeof(freebob_sample_t);
1395         printEnter();
1396         assert(dev);
1397        
1398         for(i=0; i < dev->nb_connections; i++) {
1399                 freebob_connection_t *connection= &(dev->connections[i]);
1400                 connection->status.frames_left+=dev->options.period_size;
1401                 // enable poll
1402                 connection->pfd->events=POLLIN;
1403         }
1404         printExit();
1405         return;
1406 }
1407
1408 static inline int
1409 freebob_streaming_xrun_detected_on_connection (freebob_device_t *dev, freebob_connection_t *connection) {
1410         assert((connection));
1411         if (connection->status.xruns>0) {
1412                 return TRUE;
1413         }
1414         return FALSE;
1415 }
1416
1417 static inline int
1418 freebob_streaming_xrun_detected (freebob_device_t *dev, freebob_connection_t **connections, int nb_connections) {
1419         int i;
1420        
1421         for(i=0; i < nb_connections; i++) {
1422                 assert((connections[i]));
1423
1424                 if (freebob_streaming_xrun_detected_on_connection (dev, connections[i])) {
1425                         return TRUE;
1426                 }
1427         }
1428         return FALSE;
1429 }
1430
1431 int freebob_streaming_start_iso_connection(freebob_device_t *dev, freebob_connection_t *connection) {
1432         int err;
1433
1434         err=0;
1435         if (connection->spec.direction == FREEBOB_CAPTURE) {
1436                 connection->status.packets=0;   
1437                 connection->status.dropped=0;           
1438                 if (connection->spec.is_master) { //master connection
1439
1440                         debugPrint(DEBUG_LEVEL_STARTUP,
1441                                 "Init ISO master receive handler on channel %d...\n",
1442                                 connection->iso.iso_channel);
1443
1444                         debugPrint(DEBUG_LEVEL_STARTUP,
1445                                 "   (%s, BUFFERS=%d, PACKET_SIZE=%d, PACKET_MAX=%d, IRQ=%d, %d PKT/PERIOD)...\n",
1446                                 (connection->iso.receive_mode==RAW1394_DMA_PACKET_PER_BUFFER ? "PACKET_PER_BUFFER" : "BUFFERFILL"),
1447                                 connection->iso.buffers,
1448                                 connection->iso.packet_size,
1449                                 connection->iso.max_packet_size,
1450                                 connection->iso.irq_interval,
1451                                 connection->iso.packets_per_period);
1452
1453                         raw1394_iso_recv_init(
1454                                         connection->raw_handle,
1455                                         iso_master_receive_handler,
1456                                         connection->iso.buffers,
1457                                         connection->iso.max_packet_size,
1458                                         connection->iso.iso_channel,
1459                                         connection->iso.receive_mode, // RAW1394_DMA_BUFFERFILL,
1460                                         connection->iso.irq_interval);
1461
1462                 } else {
1463                         //slave receive connection
1464                         debugPrint(DEBUG_LEVEL_STARTUP,
1465                                 "Init ISO slave receive handler on channel %d...\n",
1466                                 connection->iso.iso_channel);
1467
1468                         debugPrint(DEBUG_LEVEL_STARTUP,
1469                                 "   (%s, BUFFERS=%d, PACKET_SIZE=%d, PACKET_MAX=%d, IRQ=%d, %d PKT/PERIOD)...\n",
1470                                 (connection->iso.receive_mode==RAW1394_DMA_PACKET_PER_BUFFER ? "PACKET_PER_BUFFER" : "BUFFERFILL"),                             connection->iso.buffers,
1471                                 connection->iso.packet_size,
1472                                 connection->iso.max_packet_size,
1473                                 connection->iso.irq_interval,
1474                                 connection->iso.packets_per_period);
1475
1476                         raw1394_iso_recv_init( 
1477                                         connection->raw_handle,
1478                                         iso_slave_receive_handler,
1479                                         connection->iso.buffers,
1480                                         connection->iso.max_packet_size,
1481                                         connection->iso.iso_channel,
1482                                         connection->iso.receive_mode, // RAW1394_DMA_BUFFERFILL,
1483                                         connection->iso.irq_interval);
1484                 }
1485                
1486                 debugPrint(DEBUG_LEVEL_STARTUP,
1487                         "Start ISO receive for connection on channel %d at cycle %d...\n",
1488                           connection->iso.iso_channel, connection->iso.startcycle);
1489                        
1490                 err = raw1394_iso_recv_start(
1491                                 connection->raw_handle,
1492                 connection->iso.startcycle,
1493                 -1,//IEC61883_TAG_WITH_CIP,
1494                 0);
1495
1496                 if (err) {
1497                         printError("couldn't start receiving: %s\n",
1498                                            strerror (errno));
1499                                 // TODO: cleanup
1500                         return err;
1501                 }                                       
1502                
1503         } else if (connection->spec.direction == FREEBOB_PLAYBACK) {
1504                
1505                 if (connection->spec.is_master) { // master connection
1506                         debugPrint(DEBUG_LEVEL_STARTUP,
1507                                 "Init ISO master transmit handler on channel %d...\n",
1508                                 connection->iso.iso_channel);
1509
1510                         debugPrint(DEBUG_LEVEL_STARTUP,
1511                                 "   (BUFFERS=%d, PACKET_SIZE=%d, PACKET_MAX=%d, IRQ=%d, %d PKT/PERIOD)...\n",
1512                                 connection->iso.buffers,
1513                                 connection->iso.packet_size,
1514                                 connection->iso.max_packet_size,
1515                                 connection->iso.irq_interval,
1516                                 connection->iso.packets_per_period);
1517
1518                                
1519                         raw1394_iso_xmit_init(
1520                                         connection->raw_handle,
1521                                         iso_master_transmit_handler,
1522                                         connection->iso.buffers,
1523                                         connection->iso.max_packet_size,
1524                                         connection->iso.iso_channel,
1525                                         RAW1394_ISO_SPEED_400,
1526                                         connection->iso.irq_interval);                 
1527                 } else {
1528                        
1529                         debugPrint(DEBUG_LEVEL_STARTUP,
1530                                 "Init ISO slave transmit handler on channel %d...\n",
1531                                 connection->iso.iso_channel);
1532
1533                         debugPrint(DEBUG_LEVEL_STARTUP,
1534                                 "   (BUFFERS=%d, PACKET_SIZE=%d, PACKET_MAX=%d, IRQ=%d, %d PKT/PERIOD)...\n",
1535                                 connection->iso.buffers,
1536                                 connection->iso.packet_size,
1537                                 connection->iso.max_packet_size,
1538                                 connection->iso.irq_interval,
1539                                 connection->iso.packets_per_period);
1540
1541                         raw1394_iso_xmit_init(
1542                                         connection->raw_handle,
1543                                         iso_slave_transmit_handler,
1544                                         connection->iso.buffers,
1545                                         connection->iso.max_packet_size,
1546                                         connection->iso.iso_channel,
1547                                         RAW1394_ISO_SPEED_400,
1548                                         connection->iso.irq_interval); 
1549
1550                 }
1551        
1552                 debugPrint(DEBUG_LEVEL_STARTUP,
1553                         "Start ISO transmit for connection on channel %d at cycle %d\n",
1554                         connection->iso.iso_channel, connection->iso.startcycle);
1555                        
1556                 err=raw1394_iso_xmit_start(
1557                         connection->raw_handle,
1558                         connection->iso.startcycle,
1559                         connection->iso.prebuffers);
1560
1561                 if (err) {
1562                         printError("couldn't start transmitting: %s\n",
1563                                            strerror (errno));
1564                                 // TODO: cleanup
1565                         return err;
1566                 }
1567         }
1568         return 0;
1569 }
1570
1571 int freebob_streaming_stop_iso_connection(freebob_device_t *dev, freebob_connection_t *connection) {
1572        
1573         debugPrintWithTimeStamp(DEBUG_LEVEL_STARTUP, "Stop connection on channel %d ...\n", connection->iso.iso_channel);
1574         if (connection->spec.direction == FREEBOB_CAPTURE) {
1575                 raw1394_iso_recv_flush(connection->raw_handle);
1576         } else {
1577                 raw1394_iso_xmit_sync(connection->raw_handle);
1578         }
1579                
1580         raw1394_iso_stop(connection->raw_handle);
1581        
1582         debugPrint(DEBUG_LEVEL_STARTUP, "Shutdown connection on channel %d ...\n", connection->iso.iso_channel);
1583                
1584         raw1394_iso_shutdown(connection->raw_handle);
1585        
1586         return 0;
1587 }
1588
1589 int freebob_streaming_wait_for_sync_stream(freebob_device_t *dev, freebob_connection_t *connection) {
1590         int err;
1591        
1592         debugPrint(DEBUG_LEVEL_STARTUP, "Waiting for the sync stream...\n");
1593        
1594         // start the sync master connection
1595        
1596         connection->iso.startcycle=-1; // don't care when we start this
1597         connection->status.events=0;
1598        
1599         freebob_streaming_start_iso_connection(dev,connection);
1600        
1601         // wait until something is received/sent on the connection
1602         while(connection->status.events==0) {
1603                 err=0;
1604                        
1605                 // get a packet on the sync master connection
1606                 err=raw1394_loop_iterate(connection->raw_handle);
1607                        
1608                 if (err == -1) {
1609                         printError("Possible raw1394 error: %s on sync master connection: %d\n",
1610                                            strerror (errno),connection->spec.id);
1611                 }
1612         }
1613        
1614         freebob_streaming_stop_iso_connection(dev,connection);
1615        
1616         // reset the connection
1617         freebob_streaming_reset_connection(dev, connection);
1618        
1619         // FIXME: only works for sync on receive stream because we don't prefill.
1620         // ?FIXED?
1621         if(connection->spec.direction==1) { // playback
1622                 int i;
1623
1624                 if((err=freebob_streaming_reset_playback_streams(dev))<0) {
1625                         printError("Could not reset playback streams.\n");
1626                         return err;
1627                 }
1628
1629                 // put nb_periods*period_size of null frames into the playback buffers
1630                 if((err=freebob_streaming_prefill_playback_streams(dev))<0) {
1631                         printError("Could not prefill playback streams.\n");
1632                         return err;
1633                 }
1634                        
1635                 // we should transfer nb_buffers periods of playback from the stream buffers to the event buffer
1636                 for (i=0;i<dev->options.nb_buffers;i++) {
1637                         freebob_streaming_transfer_playback_buffers(dev);
1638                 }
1639         }       
1640
1641         debugPrint(DEBUG_LEVEL_STARTUP, "  stream is running.\n");
1642
1643         return 0;
1644 }
1645
1646 int freebob_streaming_start_iso(freebob_device_t *dev) {
1647         freebob_connection_t *connection=NULL;
1648         unsigned int c;
1649 //      int err;
1650
1651         // when starting the thread, we should wait with iterating the slave connections
1652         // until the master connection has processed some samples, because the devices
1653         // tend to start with a stream of no-data packets, leading to xruns on slave
1654         // transmit, leading to multiple restarts of the thread, which doesn't work correctly (yet)
1655 /*      debugPrint(DEBUG_LEVEL_STARTUP, "Waiting for the sync master...\n");
1656        
1657        
1658         connection=dev->sync_master_connection;
1659        
1660         // start the sync master connection
1661        
1662         connection->iso.startcycle=-1; // don't care when we start this
1663         connection->status.events=0;
1664        
1665         freebob_streaming_start_iso_connection(dev,connection);*/
1666        
1667         // wait until something is received/sent on the connection
1668 //      while(connection->status.events==0) {
1669 //              err=0;
1670 //             
1671 //              // get a packet on the sync master connection
1672 //              err=raw1394_loop_iterate(connection->raw_handle);
1673 //             
1674 //              if (err == -1) {
1675 //                      printError("Possible raw1394 error: %s on sync master connection: %d\n",
1676 //                                         strerror (errno),connection->spec.id);
1677 //              }
1678 //      }
1679        
1680 #define NB_CYCLES_TO_SKIP 100
1681
1682         // get the last timestamp & calculate the start times for the connections
1683         // start ISO xmit/receive
1684        
1685         // we introduce some delay to let the freebob devices start streaming.
1686         usleep(2000);
1687         freebob_streaming_wait_for_sync_stream(dev, dev->sync_master_connection);
1688        
1689         for(c=0; c < dev->nb_connections; c++) {
1690                 connection= &(dev->connections[c]);
1691                
1692 //              if(connection != dev->sync_master_connection) {
1693                        
1694                         connection->iso.startcycle=(dev->sync_master_connection->status.last_timestamp.cycle+NB_CYCLES_TO_SKIP)%8000;
1695                        
1696 //                      connection->iso.startcycle=0;
1697                        
1698                         freebob_streaming_start_iso_connection(dev, connection);
1699 //              }       
1700         }       
1701        
1702         return 0;
1703 }
1704
1705 int freebob_streaming_stop_iso(freebob_device_t *dev) {
1706         unsigned int c;
1707         // stop ISO xmit/receive
1708         for(c=0; c < dev->nb_connections; c++) {
1709                 freebob_connection_t *connection= &(dev->connections[c]);
1710
1711                 freebob_streaming_stop_iso_connection(dev,connection);
1712         }
1713         return 0;
1714 }
1715
1716 /*
1717  * The thread responsible for packet iteration
1718  */
1719
1720 void * freebob_iso_packet_iterator(void *arg)
1721 {
1722
1723         freebob_device_t * dev=(freebob_device_t *) arg;
1724         int err;
1725 //      int cycle=0;
1726         int underrun_detected=0;
1727        
1728         int notdone=TRUE;
1729        
1730         freebob_connection_t *connection=NULL;
1731        
1732         assert(dev);
1733         assert(dev->sync_master_connection);
1734         assert(dev->connections);
1735        
1736         debugPrint(DEBUG_LEVEL_STARTUP, "Entering packetizer thread...\n");
1737        
1738         freebob_streaming_start_iso(dev);
1739
1740         freebob_streaming_print_bufferfill(dev);
1741         // start xmit/receive
1742         debugPrint(DEBUG_LEVEL_STARTUP, "Go Go Go!!!\n");
1743        
1744 #define POLL_BASED     
1745 #ifdef POLL_BASED
1746         while (dev->packetizer.run && !underrun_detected) {
1747                
1748                 freebob_streaming_period_reset(dev);
1749                
1750                 dev->watchdog_check = 1;
1751                
1752                 notdone=TRUE;
1753                
1754                 while(notdone) {
1755                        
1756                         err = poll (dev->pfds, dev->nfds, -1);
1757                        
1758                         if (err == -1) {
1759                                 if (errno == EINTR) {
1760                                         continue;
1761                                 }
1762                                 printError("poll error: %s\n", strerror (errno));
1763                                 dev->packetizer.status = -2;
1764                                 dev->packetizer.retval = 0;
1765                                 notdone=FALSE;
1766                                 break;
1767                         }
1768                        
1769                         int i=0;
1770                
1771                         for (i = 0; i < dev->nfds; i++) {
1772                                 if (dev->pfds[i].revents & POLLERR) {
1773                                         printError ("error on fd for %d\n",i);
1774                                 }
1775
1776                                 if (dev->pfds[i].revents & POLLHUP) {
1777                                         printError ("hangup on fd for %d\n",i);
1778                                 }
1779                                
1780                                 if(dev->pfds[i].revents & (POLLIN)) {
1781                                         // FIXME: this can segfault
1782                                         connection=dev->fdmap[i];
1783                                        
1784                                         assert(connection);
1785                                        
1786                                         err = raw1394_loop_iterate (connection->raw_handle);
1787                                        
1788                                         // detect underruns on the connection
1789                                         if (freebob_streaming_xrun_detected_on_connection(dev,connection)) {
1790                                                 printError("Xrun on connection %d\n", connection->spec.id);
1791                                                 underrun_detected=TRUE;
1792                                                 break; // we can exit as the underrun handler will flush the buffers anyway
1793                                         }
1794                                
1795                                         if (err == -1) {
1796                                                 printError ("possible raw1394 error: %s\n", strerror (errno));
1797                                                 dev->packetizer.status = -2;
1798                                                 dev->packetizer.retval = 0;
1799                                                 notdone=FALSE;
1800                                                 break;
1801                                         }
1802                                 }
1803
1804                                
1805
1806                         }
1807
1808                         notdone=(!freebob_streaming_period_complete(dev) && !underrun_detected && (dev->packetizer.run)) && notdone;
1809                 }
1810                
1811                 if(underrun_detected) {
1812                         dev->xrun_detected=TRUE;
1813                 }
1814
1815 #else
1816         while (dev->packetizer.run && !underrun_detected) {
1817                
1818                 freebob_streaming_period_reset(dev);
1819                        
1820                 dev->watchdog_check = 1;
1821                        
1822                 notdone=TRUE;
1823                        
1824                 while(notdone) {
1825                                
1826                         err=0;
1827                        
1828                         // this makes sure the sync_master finishes before the others finish
1829                         connection=dev->sync_master_connection;
1830                         if (connection->status.frames_left > 0) {
1831                                 // process a packet on the sync master connection
1832                                 err=raw1394_loop_iterate(connection->raw_handle);
1833                                
1834                                 if (err == -1) {
1835                                         printError("Possible raw1394 error: %s on sync master connection: %d\n",
1836                                                         strerror (errno),connection->spec.id);
1837                                                
1838                                         dev->packetizer.status = -2;
1839                                         dev->packetizer.retval = 0;
1840                                         notdone=FALSE;
1841                                         break;
1842                                 }
1843                                
1844                                 // detect underruns on the sync master connection
1845                                 if (freebob_streaming_xrun_detected(dev,&connection,1)) {
1846                                         printError("Xrun on sync master connection %d\n", connection->spec.id);
1847                                         underrun_detected=TRUE;
1848                                         break; // we can exit as the underrun handler will flush the buffers anyway
1849                                 }
1850                         }
1851                                
1852                        
1853                         // now iterate on the slave connections
1854                         int c;
1855                         for(c=0; c<dev->nb_connections; c++) {
1856
1857                                 connection = &(dev->connections[c]);
1858                                
1859                                 if ((connection == dev->sync_master_connection))
1860                                                 continue;
1861                                
1862                                 err=raw1394_loop_iterate(connection->raw_handle);
1863                                
1864                                 if (err == -1) {
1865                                         printError("Possible raw1394 error: %s on connection: %d\n",
1866                                                         strerror (errno),connection->spec.id);
1867                                         dev->packetizer.status = -2;
1868                                         dev->packetizer.retval = 0;
1869                                         notdone=FALSE;
1870                                 }
1871                                
1872                                 // detect underruns
1873                                 if (freebob_streaming_xrun_detected(dev,&connection,1)) {
1874                                         printError("Xrun on slave connection %d\n", connection->spec.id);
1875                                         underrun_detected=TRUE;
1876                                         break; // we can exit as the underrun handler will flush the buffers anyway
1877                                 }
1878                         }
1879                                
1880                         notdone=(!freebob_streaming_period_complete(dev) && !underrun_detected && (dev->packetizer.run)) && notdone;
1881                 }
1882                
1883                 if(underrun_detected) {
1884                         dev->xrun_detected=TRUE;
1885                 }
1886        
1887 #endif
1888                 // notify the waiting thread
1889 #ifdef DEBUG
1890                 debugPrint(DEBUG_LEVEL_HANDLERS, "Post semaphore ");
1891                 int c;
1892                 for(c=0; c<dev->nb_connections; c++) {
1893                         connection = &(dev->connections[c]);
1894                         debugPrintShort(DEBUG_LEVEL_HANDLERS,"[%d: %d]",c,connection->status.frames_left);
1895                 }
1896                 debugPrintShort(DEBUG_LEVEL_HANDLERS,"\n");
1897 #endif
1898
1899                 sem_post(&dev->packetizer.transfer_boundary);
1900
1901 #ifdef DEBUG
1902                 if((dev->sync_master_connection->status.packets - dev->sync_master_connection->status.total_packets_prev) > 1024*2) {
1903                         unsigned int i;
1904                         debugPrintShort(DEBUG_LEVEL_PACKETCOUNTER,"\r -> ");
1905                        
1906                         for(i=0; i < dev->nb_connections; i++) {
1907                                 freebob_connection_t *connection= &(dev->connections[i]);
1908                                 assert(connection);
1909                                
1910                                 debugPrintShort(DEBUG_LEVEL_PACKETCOUNTER,"[%s, %02d, %10d, %04d, %4d, (R: %04d)]",
1911                                         (connection->spec.direction==FREEBOB_CAPTURE ? "C" : "P"),
1912                                         connection->status.fdf,
1913                                         connection->status.packets,
1914                                         connection->status.last_cycle,
1915                                         connection->status.dropped,
1916                                         freebob_ringbuffer_read_space(connection->event_buffer)/(sizeof(quadlet_t)*connection->spec.dimension)
1917                                    );
1918                         }
1919        
1920                         debugPrintShort(DEBUG_LEVEL_PACKETCOUNTER," XRUNS (%2d)",dev->xrun_count);     
1921                         fflush (stderr);
1922                        
1923                         dev->sync_master_connection->status.total_packets_prev=dev->sync_master_connection->status.packets;
1924                 }
1925
1926 #endif
1927
1928         }
1929        
1930         freebob_streaming_stop_iso(dev);
1931        
1932         debugPrint(DEBUG_LEVEL_STARTUP, "Exiting packetizer thread...\n");
1933        
1934        
1935         pthread_exit (0);       
1936 }
1937
1938 void freebob_streaming_append_master_timestamp(freebob_device_t *dev, freebob_timestamp_t *t) {
1939         int c;
1940 //      int retval;
1941         freebob_connection_t *connection;
1942        
1943         assert(dev);
1944        
1945         for(c=0; c<dev->nb_connections; c++) {
1946
1947                 connection = &(dev->connections[c]);
1948                
1949                 // skip the sync master
1950                 if ((connection == dev->sync_master_connection))
1951                         continue;
1952                
1953                 // write the timestamp
1954
1955 /*              retval=freebob_ringbuffer_write(connection->timestamp_buffer,(char *)t,sizeof(freebob_timestamp_t));
1956                
1957                 if(retval!=sizeof(unsigned int)) {
1958                         printError("Timestamp buffer overrun on connection %d!\n",c);
1959                 }
1960 */             
1961         }
1962 }
1963
1964 inline int freebob_streaming_decode_midi(freebob_connection_t *connection,
1965                                                                   quadlet_t* events,
1966                                                                   unsigned int nsamples,
1967                                                                   unsigned int dbc
1968                                                                  ) {
1969         quadlet_t *target_event;
1970        
1971         assert (connection);
1972         assert (events);
1973
1974         freebob_stream_t *stream;
1975        
1976         unsigned int j=0;
1977         unsigned int s=0;
1978         int written=0;
1979         quadlet_t *buffer;
1980        
1981         for (s=0;s<connection->nb_streams;s++) {
1982                 stream=&connection->streams[s];
1983                
1984                 assert (stream);
1985                 assert (stream->spec.position < connection->spec.dimension);
1986                 assert(stream->user_buffer);
1987                
1988                 if (stream->spec.format == IEC61883_STREAM_TYPE_MIDI) {
1989                         /* idea:
1990                         spec says: current_midi_port=(dbc+j)%8;
1991                         => if we start at (dbc+stream->location-1)%8 [due to location_min=1],
1992                         we'll start at the right event for the midi port.
1993                         => if we increment j with 8, we stay at the right event.
1994                         */
1995                         buffer=((quadlet_t *)(stream->user_buffer));
1996                         written=0;
1997                        
1998                         for(j = (dbc & 0x07)+stream->spec.location-1; j < nsamples; j += 8) {
1999                                 target_event=(quadlet_t *)(events + ((j * connection->spec.dimension) + stream->spec.position));
2000                                 quadlet_t sample_int=ntohl(*target_event);
2001                                 if(IEC61883_AM824_GET_LABEL(sample_int) != IEC61883_AM824_LABEL_MIDI_NO_DATA) {
2002                                         *(buffer)=(sample_int >> 16);
2003                                         buffer++;
2004                                         written++;
2005                                 }
2006                         }
2007                        
2008                         int written_to_rb=freebob_ringbuffer_write(stream->buffer, (char *)(stream->user_buffer), written*sizeof(quadlet_t))/sizeof(quadlet_t);
2009                         if(written_to_rb<written) {
2010                                 printMessage("MIDI OUT bytes lost (%d/%d)",written_to_rb,written);
2011                         }
2012                 }
2013         }
2014         return 0;
2015        
2016 }
2017
2018 /*
2019  * according to the MIDI over 1394 spec, we are only allowed to send a maximum of one midi byte every 320usec
2020  * therefore we can only send one byte on 1 out of 3 iso cycles (=325usec)
2021  */
2022
2023 inline int freebob_streaming_encode_midi(freebob_connection_t *connection,
2024                                                                 quadlet_t* events,
2025                                                                 unsigned int nsamples,
2026                                                                 unsigned int dbc
2027                                                                 ) {
2028         quadlet_t *target_event;
2029        
2030         assert (connection);
2031         assert (events);
2032
2033         freebob_stream_t *stream;
2034
2035         unsigned int j=0;
2036         unsigned int s=0;
2037         int read=0;
2038         quadlet_t *buffer;
2039        
2040         for (s=0;s<connection->nb_streams;s++) {
2041                 stream=&connection->streams[s];
2042
2043                 assert (stream);
2044                 assert (stream->spec.position < connection->spec.dimension);
2045                 assert(stream->user_buffer);
2046
2047                 if (stream->spec.format == IEC61883_STREAM_TYPE_MIDI) {
2048                         // first prefill the buffer with NO_DATA's on all time muxed channels
2049                         for(j=0; (j < nsamples); j++) {
2050                                 target_event=(quadlet_t *)(events + ((j * connection->spec.dimension) + stream->spec.position));
2051                                
2052                                 *target_event=htonl(IEC61883_AM824_SET_LABEL(0,IEC61883_AM824_LABEL_MIDI_NO_DATA));
2053
2054                         }
2055                        
2056                         /* idea:
2057                                 spec says: current_midi_port=(dbc+j)%8;
2058                                 => if we start at (dbc+stream->location-1)%8 [due to location_min=1],
2059                                 we'll start at the right event for the midi port.
2060                                 => if we increment j with 8, we stay at the right event.
2061                         */
2062                        
2063                         if(stream->midi_counter<=0) { // we can send a byte
2064                                 read=freebob_ringbuffer_read(stream->buffer, (char *)(stream->user_buffer), 1*sizeof(quadlet_t))/sizeof(quadlet_t);
2065                                 if(read) {
2066 //                                      j = (dbc%8)+stream->spec.location-1;
2067                                         j = (dbc & 0x07)+stream->spec.location-1;
2068                                         target_event=(quadlet_t *)(events + ((j * connection->spec.dimension) + stream->spec.position));
2069                                         buffer=((quadlet_t *)(stream->user_buffer));
2070                                
2071                                         *target_event=htonl(IEC61883_AM824_SET_LABEL((*buffer)<<16,IEC61883_AM824_LABEL_MIDI_1X));
2072                                         stream->midi_counter=3; // only if we send a byte, we reset the counter
2073                                 }
2074                         } else {
2075                                 stream->midi_counter--;
2076                         }
2077                 }
2078         }
2079         return 0;
2080
2081 }
2082
2083
2084 /**
2085  * ISO send/receive callback handlers
2086  */
2087
2088 static enum raw1394_iso_disposition
2089 iso_master_receive_handler(raw1394handle_t handle, unsigned char *data,
2090         unsigned int length, unsigned char channel,
2091         unsigned char tag, unsigned char sy, unsigned int cycle,
2092         unsigned int dropped)
2093 {
2094         enum raw1394_iso_disposition retval=RAW1394_ISO_OK;
2095
2096         freebob_connection_t *connection=(freebob_connection_t *) raw1394_get_userdata (handle);
2097         assert(connection);
2098        
2099         struct iec61883_packet *packet = (struct iec61883_packet *) data;
2100         assert(packet);
2101        
2102         // FIXME: dropped packets are very bad when transmitting and the other side is sync'ing on that!
2103         connection->status.dropped+=dropped;
2104
2105 #ifdef DEBUG   
2106         connection->status.last_cycle=cycle;
2107 #endif
2108
2109         if((packet->fmt == 0x10) && (packet->fdf != 0xFF) && (packet->dbs>0) && (length>=2*sizeof(quadlet_t))) {
2110                 unsigned int nevents=((length / sizeof (quadlet_t)) - 2)/packet->dbs;
2111                
2112                 // add the data payload to the ringbuffer
2113
2114                 assert(connection->spec.dimension == packet->dbs);
2115                
2116                 if (freebob_ringbuffer_write(
2117                                 connection->event_buffer,(char *)(data+8),
2118                                 nevents*sizeof(quadlet_t)*connection->spec.dimension) <
2119                                 nevents*sizeof(quadlet_t)*connection->spec.dimension)
2120                 {
2121                         printError("MASTER RCV: Buffer overrun!\n");
2122                         connection->status.xruns++;
2123                         retval=RAW1394_ISO_DEFER;
2124                 } else {
2125                         retval=RAW1394_ISO_OK;
2126                         // we cannot offload midi encoding due to the need for a dbc value
2127                         freebob_streaming_decode_midi(connection,(quadlet_t *)(data+8), nevents, packet->dbc);
2128                 }
2129                
2130                 // keep the frame counter
2131                 connection->status.frames_left -= nevents;
2132                
2133                 // keep track of the total amount of events received
2134                 connection->status.events+=nevents;
2135
2136 #ifdef DEBUG   
2137                 connection->status.fdf=packet->fdf;
2138 #endif
2139                
2140         } else {
2141                 // discard packet
2142                 // can be important for sync though
2143         }
2144
2145         // one packet received
2146         connection->status.packets++;
2147
2148 #ifdef DEBUG
2149         if(packet->dbs) {
2150                 unsigned int nevents=((length / sizeof (quadlet_t)) - 2)/packet->dbs;
2151                 debugPrintWithTimeStamp(DEBUG_LEVEL_HANDLERS_LOWLEVEL,
2152                         "MASTER RCV: %08d, CH = %d, FDF = %X. SYT = %6d, DBS = %3d, DBC = %3d, FMT = %3d, LEN = %4d (%2d), DROPPED = %4d, info->packets=%4d, events=%4d, %d, %d\n",
2153                         cycle,
2154                         channel, packet->fdf,packet->syt,packet->dbs,packet->dbc,packet->fmt, length,
2155                         ((length / sizeof (quadlet_t)) - 2)/packet->dbs, dropped,
2156                         connection->status.packets, connection->status.events, connection->status.frames_left,
2157                         nevents);
2158         }
2159 #endif
2160
2161         if((connection->status.frames_left<=0)) {
2162                 connection->pfd->events=0;
2163                 return RAW1394_ISO_DEFER;
2164         }
2165
2166         return retval;
2167 }
2168
2169
2170 static enum raw1394_iso_disposition
2171                 iso_slave_receive_handler(raw1394handle_t handle, unsigned char *data,
2172                                                                   unsigned int length, unsigned char channel,
2173                                                                   unsigned char tag, unsigned char sy, unsigned int cycle,
2174                                                                   unsigned int dropped)
2175 {
2176         enum raw1394_iso_disposition retval=RAW1394_ISO_OK;
2177         /* slave receive is easy if you assume that the connections are synced
2178         Synced connections have matched data rates, so just receiving and
2179         calling the rcv handler would suffice in this case. As the connection
2180         is externally matched to the master connection, the buffer fill will be ok.
2181         */
2182         /* TODO: implement correct SYT behaviour */
2183                
2184         freebob_connection_t *connection=(freebob_connection_t *) raw1394_get_userdata (handle);
2185         assert(connection);
2186        
2187         struct iec61883_packet *packet = (struct iec61883_packet *) data;
2188         assert(packet);
2189        
2190         // FIXME: dropped packets are very bad when transmitting and the other side is sync'ing on that!
2191         //connection->status.packets+=dropped;
2192         connection->status.dropped+=dropped;
2193
2194 #ifdef DEBUG   
2195         connection->status.last_cycle=cycle;
2196 #endif
2197
2198         if((packet->fmt == 0x10) && (packet->fdf != 0xFF) && (packet->dbs>0) && (length>=2*sizeof(quadlet_t))) {
2199                 unsigned int nevents=((length / sizeof (quadlet_t)) - 2)/packet->dbs;
2200
2201 #ifdef DEBUG   
2202                 connection->status.fdf=packet->fdf;
2203 #endif
2204                
2205                 // add the data payload to the ringbuffer
2206
2207                 assert(connection->spec.dimension == packet->dbs);
2208
2209                 if (freebob_ringbuffer_write(
2210                                         connection->event_buffer,(char *)(data+8),
2211                                         nevents*sizeof(quadlet_t)*connection->spec.dimension) <
2212                         nevents*sizeof(quadlet_t)*connection->spec.dimension)
2213                 {
2214                         printError("SLAVE RCV: Buffer overrun!\n");
2215                         connection->status.xruns++;
2216                         retval=RAW1394_ISO_DEFER;
2217                 } else {
2218                         retval=RAW1394_ISO_OK;
2219                         // we cannot offload midi encoding due to the need for a dbc value
2220                         freebob_streaming_decode_midi(connection,(quadlet_t *)(data+8), nevents, packet->dbc);
2221                 }
2222
2223                 connection->status.frames_left-=nevents;
2224                
2225                 // keep track of the total amount of events received
2226                 connection->status.events+=nevents;
2227         } else {
2228                 // discard packet
2229                 // can be important for sync though
2230         }
2231
2232         // one packet received
2233         connection->status.packets++;
2234        
2235         if(packet->dbs) {
2236                 debugPrintWithTimeStamp(DEBUG_LEVEL_HANDLERS_LOWLEVEL,
2237                         "SLAVE RCV: CH = %d, FDF = %X. SYT = %6d, DBS = %3d, DBC = %3d, FMT = %3d, LEN = %4d (%2d), DROPPED = %6d\n",
2238                         channel, packet->fdf,
2239                         packet->syt,
2240                         packet->dbs,
2241                         packet->dbc,
2242                         packet->fmt,
2243                         length,
2244                         ((length / sizeof (quadlet_t)) - 2)/packet->dbs, dropped);
2245         }
2246                
2247                
2248         if((connection->status.frames_left<=0)) {
2249                 connection->pfd->events=0;
2250                 return RAW1394_ISO_DEFER;
2251         }
2252
2253         return retval;
2254 }
2255
2256 /**
2257  * The master transmit handler.
2258  *
2259  * This is the most difficult, because we have to generate timing information ourselves.
2260  *
2261  */
2262
2263 /*
2264  Includes code from libiec61883
2265  */
2266 static enum raw1394_iso_disposition
2267 iso_master_transmit_handler(raw1394handle_t handle,
2268                 unsigned char *data, unsigned int *length,
2269                 unsigned char *tag, unsigned char *sy,
2270                 int cycle, unsigned int dropped)
2271 {
2272         freebob_connection_t *connection=(freebob_connection_t *) raw1394_get_userdata (handle);
2273         assert(connection);
2274        
2275         struct iec61883_packet *packet = (struct iec61883_packet *) data;
2276         assert(packet);
2277         assert(length);
2278         assert(tag);
2279         assert(sy);     
2280        
2281         // construct the packet cip
2282         int nevents = iec61883_cip_fill_header (handle, &connection->status.cip, packet);
2283         int nsamples=0;
2284         int bytes_read;
2285        
2286 #ifdef DEBUG   
2287         connection->status.last_cycle=cycle;
2288
2289         if(packet->fdf != 0xFF) {
2290                 connection->status.fdf=packet->fdf;
2291         }
2292 #endif
2293
2294         enum raw1394_iso_disposition retval = RAW1394_ISO_OK;
2295
2296
2297
2298         if (nevents > 0) {
2299                 nsamples = nevents;
2300         }
2301         else {
2302                 if (connection->status.cip.mode == IEC61883_MODE_BLOCKING_EMPTY) {
2303                         nsamples = 0;
2304                 }
2305                 else {
2306                         nsamples = connection->status.cip.syt_interval;
2307                 }
2308         }
2309        
2310         // dropped packets are very bad when transmitting and the other side is sync'ing on that!
2311         connection->status.dropped += dropped;
2312                
2313         if (nsamples > 0) {
2314
2315                 assert(connection->spec.dimension == packet->dbs);
2316
2317                 if ((bytes_read=freebob_ringbuffer_read(connection->event_buffer,(char *)(data+8),nsamples*sizeof(quadlet_t)*connection->spec.dimension)) <
2318                                    nsamples*sizeof(quadlet_t)*connection->spec.dimension)
2319                 {
2320                         printError("MASTER XMT: Buffer underrun! (%d / %d) (%d / %d )\n",
2321                                            bytes_read,nsamples*sizeof(quadlet_t)*connection->spec.dimension,
2322                                            freebob_ringbuffer_read_space(connection->event_buffer),0);
2323                         connection->status.xruns++;
2324                         retval=RAW1394_ISO_DEFER;
2325                         nsamples=0;
2326                 } else {
2327                         retval=RAW1394_ISO_OK;
2328                         // we cannot offload midi encoding due to the need for a dbc value
2329                         freebob_streaming_encode_midi(connection,(quadlet_t *)(data+8), nevents, packet->dbc);
2330                 }
2331         }
2332        
2333         *length = nsamples * connection->spec.dimension * sizeof (quadlet_t) + 8;
2334         *tag = IEC61883_TAG_WITH_CIP;
2335         *sy = 0;
2336        
2337         // keep track of the total amount of events transmitted
2338         connection->status.events+=nsamples;
2339        
2340         connection->status.frames_left-=nsamples;
2341                
2342         // one packet transmitted
2343         connection->status.packets++;
2344
2345         if(packet->dbs) {
2346                 debugPrintWithTimeStamp(DEBUG_LEVEL_HANDLERS_LOWLEVEL,
2347                                                                 "MASTER XMT: %08d, CH = %d, FDF = %X. SYT = %6d, DBS = %3d, DBC = %3d, FMT = %3d, LEN = %4d (%2d), DROPPED = %4d, info->packets=%4d, events=%4d, %d, %d %d\n", cycle,
2348                                                                 connection->iso.iso_channel,  packet->fdf,packet->syt,packet->dbs,packet->dbc,packet->fmt, *length,((*length / sizeof (quadlet_t)) - 2)/packet->dbs, dropped,
2349                                                                 connection->status.packets, connection->status.events, connection->status.frames_left,
2350                                                                 nevents, nsamples);
2351         }
2352
2353         if((connection->status.frames_left<=0)) {
2354                 connection->pfd->events=0;
2355                 return RAW1394_ISO_DEFER;
2356         }
2357
2358         return retval;
2359
2360 }
2361
2362 static enum raw1394_iso_disposition
2363                 iso_slave_transmit_handler(raw1394handle_t handle,
2364                                                                    unsigned char *data, unsigned int *length,
2365                                                                    unsigned char *tag, unsigned char *sy,
2366                                                                    int cycle, unsigned int dropped)
2367 {
2368         freebob_connection_t *connection=(freebob_connection_t *) raw1394_get_userdata (handle);
2369         assert(connection);
2370        
2371         struct iec61883_packet *packet = (struct iec61883_packet *) data;
2372         assert(packet);
2373         assert(length);
2374         assert(tag);
2375         assert(sy);     
2376        
2377         // construct the packet cip
2378         struct iec61883_cip old_cip;
2379         memcpy(&old_cip,&connection->status.cip,sizeof(struct iec61883_cip));
2380        
2381         int nevents = iec61883_cip_fill_header (handle, &connection->status.cip, packet);
2382         int nsamples=0;
2383         int bytes_read;
2384
2385 #ifdef DEBUG   
2386         connection->status.last_cycle=cycle;
2387
2388         if(packet->fdf != 0xFF) {
2389                 connection->status.fdf=packet->fdf;
2390         }
2391 #endif
2392
2393         enum raw1394_iso_disposition retval = RAW1394_ISO_OK;
2394
2395
2396         if (nevents > 0) {
2397                 nsamples = nevents;
2398         }
2399         else {
2400                 if (connection->status.cip.mode == IEC61883_MODE_BLOCKING_EMPTY) {
2401                         nsamples = 0;
2402                 }
2403                 else {
2404                         nsamples = connection->status.cip.syt_interval;
2405                 }
2406         }
2407        
2408         // dropped packets are very bad when transmitting and the other side is sync'ing on that!
2409         //connection->status.packets+=dropped;
2410         connection->status.dropped += dropped;
2411                
2412         if (nsamples > 0) {
2413                 int bytes_to_read=nsamples*sizeof(quadlet_t)*connection->spec.dimension;
2414
2415                 assert(connection->spec.dimension == packet->dbs);
2416
2417                 if ((bytes_read=freebob_ringbuffer_read(connection->event_buffer,(char *)(data+8),bytes_to_read)) <
2418                         bytes_to_read)
2419                 {
2420                         /* there is no more data in the ringbuffer */
2421                        
2422                         /* If there are already more than on period
2423                         * of frames transfered to the XMIT buffer, there is no xrun.
2424                         *
2425                         */
2426                         if(connection->status.frames_left<=0) {
2427                                 // we stop processing this untill the next period boundary
2428                                 // that's when new data is ready
2429                                
2430                                 connection->pfd->events=0;
2431                                
2432                                 // reset the cip to the old value
2433                                 memcpy(&connection->status.cip,&old_cip,sizeof(struct iec61883_cip));
2434
2435                                 // retry this packet
2436                                 retval=RAW1394_ISO_AGAIN;
2437 //                              retval=RAW1394_ISO_DEFER;
2438                                 nsamples=0;
2439                         } else {
2440                                 printError("SLAVE XMT : Buffer underrun! %d (%d / %d) (%d / %d )\n",
2441                                            connection->status.frames_left, bytes_read,bytes_to_read,
2442                                            freebob_ringbuffer_read_space(connection->event_buffer),0);
2443                                 connection->status.xruns++;
2444                                 retval=RAW1394_ISO_DEFER;
2445                                 nsamples=0;
2446                         }
2447                 } else {
2448                         retval=RAW1394_ISO_OK;
2449                         // we cannot offload midi encoding due to the need for a dbc value
2450                         freebob_streaming_encode_midi(connection,(quadlet_t *)(data+8), nevents, packet->dbc);
2451                 }
2452         }
2453        
2454         *length = nsamples * connection->spec.dimension * sizeof (quadlet_t) + 8;
2455         *tag = IEC61883_TAG_WITH_CIP;
2456         *sy = 0;
2457        
2458         // keep track of the total amount of events transmitted
2459         connection->status.events+=nsamples;
2460        
2461         connection->status.frames_left-=nsamples;
2462                
2463         // one packet transmitted
2464         connection->status.packets++;
2465
2466         if(packet->dbs) {
2467                 debugPrintWithTimeStamp(DEBUG_LEVEL_HANDLERS_LOWLEVEL,
2468                                                                 "SLAVE XMT : %08d, CH = %d, FDF = %X. SYT = %6d, DBS = %3d, DBC = %3d, FMT = %3d, LEN = %4d (%2d), DROPPED = %4d, info->packets=%4d, events=%4d, %d, %d %d\n", cycle,
2469                                                                 connection->iso.iso_channel,  packet->fdf,packet->syt,packet->dbs,packet->dbc,packet->fmt, *length,((*length / sizeof (quadlet_t)) - 2)/packet->dbs, dropped,
2470                                                                 connection->status.packets, connection->status.events, connection->status.frames_left,
2471                                                                 nevents, nsamples);
2472         }
2473
2474         if((connection->status.frames_left<=0)) {
2475                 connection->pfd->events=0;
2476                 return RAW1394_ISO_DEFER;
2477         }
2478
2479         return retval;
2480
2481 }
2482
2483 /*
2484  * Decoders and encoders
2485  */
2486 #ifdef ENABLE_SSE
2487 typedef float v4sf __attribute__ ((vector_size (16)));
2488 typedef int v4si __attribute__ ((vector_size (16)));
2489 typedef int v2si __attribute__ ((vector_size (8)));
2490
2491 static inline int 
2492 freebob_decode_events_to_stream(freebob_connection_t *connection,
2493                                                                 freebob_stream_t *stream,
2494                                                                 quadlet_t* events,
2495                                                                 unsigned int nsamples,
2496                                                                 unsigned int dbc
2497                                                                 ) {
2498         quadlet_t *target_event;
2499         int do_ringbuffer_write=0;
2500        
2501         assert (stream);
2502         assert (connection);
2503         assert (events);
2504         assert (stream->spec.position < connection->spec.dimension);
2505                
2506         freebob_sample_t *buffer=NULL;
2507         float *floatbuff=NULL;
2508
2509         int dimension=connection->spec.dimension;
2510        
2511         static const float multiplier = 1.0f / (float)(0x7FFFFF);
2512         static const float sse_multiplier[4] __attribute__((aligned(16))) = {
2513                 1.0f / (float)(0x7FFFFF),
2514                 1.0f / (float)(0x7FFFFF),
2515                 1.0f / (float)(0x7FFFFF),
2516                 1.0f / (float)(0x7FFFFF)
2517         };
2518         unsigned int tmp[4];
2519
2520         assert(stream->user_buffer);
2521        
2522         switch(stream->buffer_type) {
2523                 case freebob_buffer_type_per_stream:
2524                 default:
2525                         // use the preallocated buffer (at init time)
2526                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2527                        
2528                         do_ringbuffer_write=1;
2529                         break;
2530                                
2531                 case freebob_buffer_type_uint24:
2532                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2533                        
2534                         do_ringbuffer_write=0;
2535                         break;
2536                                
2537                 case freebob_buffer_type_float:
2538                         floatbuff=((float *)(stream->user_buffer))+stream->user_buffer_position;
2539                        
2540                         do_ringbuffer_write=0;
2541                         break;
2542         }
2543        
2544         int j=0;
2545         int written=0;
2546        
2547         if (stream->spec.format== IEC61883_STREAM_TYPE_MBLA) {
2548                 target_event=(quadlet_t *)(events + stream->spec.position);
2549
2550                 // TODO: convert this into function pointer based
2551                 switch(stream->buffer_type) {
2552                         default:
2553                         case freebob_buffer_type_uint24:
2554                                 for(j = 0; j < nsamples; j += 1) { // decode max nsamples
2555                                         *(buffer)=(ntohl((*target_event) ) & 0x00FFFFFF);
2556                                         buffer++;
2557                                         target_event+=dimension;
2558                                 }
2559                                 break;
2560                         case freebob_buffer_type_float:
2561                                 j=0;
2562                                 if(nsamples>3) {
2563                                         for(j = 0; j < nsamples-3; j += 4) {
2564                                                 tmp[0] = ntohl(*target_event);
2565                                                 target_event += dimension;
2566                                                 tmp[1] = ntohl(*target_event);
2567                                                 target_event += dimension;
2568                                                 tmp[2] = ntohl(*target_event);
2569                                                 target_event += dimension;
2570                                                 tmp[3] = ntohl(*target_event);
2571                                                 target_event += dimension;
2572                                                 asm("pslld $8, %[in2]\n\t" // sign extend 24th bit
2573                                                                 "pslld $8, %[in1]\n\t"
2574                                                                 "psrad $8, %[in2]\n\t"
2575                                                                 "psrad $8, %[in1]\n\t"
2576                                                                 "cvtpi2ps %[in2], %%xmm0\n\t"
2577                                                                 "movlhps %%xmm0, %%xmm0\n\t"
2578                                                                 "cvtpi2ps %[in1], %%xmm0\n\t"
2579                                                                 "mulps %[ssemult], %%xmm0\n\t"
2580                                                                 "movups %%xmm0, %[floatbuff]"
2581                                                         : [floatbuff] "=m" (*(v4sf*)floatbuff)
2582                                                         : [in1] "y" (*(v2si*)tmp),
2583                                                 [in2] "y" (*(v2si*)(tmp+2)),
2584                                                 [ssemult] "x" (*(v4sf*)sse_multiplier)
2585                                                         : "xmm0");
2586                                                 floatbuff += 4;
2587                                         }
2588                                 }
2589                                 for(; j < nsamples; ++j) { // decode max nsamples
2590                                         unsigned int v = ntohl(*target_event) & 0x00FFFFFF;
2591                                         // sign-extend highest bit of 24-bit int
2592                                         int tmp = (int)(v << 8) / 256;
2593                                         *floatbuff = tmp * multiplier;
2594
2595                                         floatbuff++;
2596                                         target_event += dimension;
2597                                 }
2598                                 asm volatile("emms");
2599                                
2600                                 break;
2601                 }
2602
2603 //              fprintf(stderr,"rb write [%02d: %08p %08p]\n",stream->spec.position, stream, stream->buffer);
2604        
2605 //      fprintf(stderr,"rb write [%02d: %08p %08p %08X, %d, %d]\n",stream->spec.position, stream, stream->buffer, *buffer, nsamples, written);
2606                 if(do_ringbuffer_write) {
2607                         // reset the buffer pointer
2608                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2609                         written=freebob_ringbuffer_write(stream->buffer, (char *)(buffer), nsamples*sizeof(freebob_sample_t))/sizeof(freebob_sample_t);
2610                 } else {
2611                         written=nsamples;
2612                 }
2613                
2614 //      fprintf(stderr,"rb write1[%02d: %08p %08p %08X, %d, %d]\n",stream->spec.position, stream, stream->buffer, *buffer, nsamples, written);
2615                
2616                 return written;
2617        
2618         } else if (stream->spec.format == IEC61883_STREAM_TYPE_MIDI) {
2619                 return nsamples; // for midi streams we always indicate a full write
2620        
2621         } else {//if (stream->spec.format == IEC61883_STREAM_TYPE_SPDIF) {
2622                 return nsamples; // for unsupported we always indicate a full read
2623        
2624         }
2625        
2626         return 0;
2627
2628
2629 }
2630
2631 static inline int
2632 freebob_encode_stream_to_events(freebob_connection_t *connection,
2633                                                                 freebob_stream_t *stream,
2634                                                                 quadlet_t* events,
2635                                                                 unsigned int nsamples,
2636                                                                 unsigned int dbc
2637                                                                 ) {
2638         quadlet_t *target_event;
2639         //freebob_sample_t buff[nsamples];
2640         int do_ringbuffer_read=0;
2641        
2642         freebob_sample_t *buffer=NULL;
2643         float *floatbuff=NULL;
2644
2645         const float multiplier = (float)(0x7FFFFF00);
2646         static const float sse_multiplier[4] __attribute__((aligned(16))) = {
2647                 (float)(0x7FFFFF00),
2648                 (float)(0x7FFFFF00),
2649                 (float)(0x7FFFFF00),
2650                 (float)(0x7FFFFF00)
2651         };
2652
2653         static const int sse_mask[4] __attribute__((aligned(16))) = {
2654                 0x40000000,  0x40000000,  0x40000000,  0x40000000
2655         };
2656
2657         unsigned int out[4];   
2658        
2659         int dimension=connection->spec.dimension;
2660        
2661         unsigned int j=0;
2662         unsigned int read=0;
2663
2664         assert (stream);
2665         assert (connection);
2666         assert (events);
2667         assert (stream->spec.position < connection->spec.dimension);
2668        
2669         switch(stream->buffer_type) {
2670                 case freebob_buffer_type_per_stream:
2671                 default:
2672 //                      assert(nsamples < dev->options.period_size);
2673                         // use the preallocated buffer (at init time)
2674                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2675                        
2676                         do_ringbuffer_read=1;
2677                         break;
2678                                
2679                 case freebob_buffer_type_uint24:
2680                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2681                        
2682                         do_ringbuffer_read=0;
2683                         break;
2684                                
2685                 case freebob_buffer_type_float:
2686                         floatbuff=((float *)(stream->user_buffer))+stream->user_buffer_position;
2687                        
2688                         do_ringbuffer_read=0;
2689                         break;
2690         }
2691        
2692        
2693         if(stream->spec.format == IEC61883_STREAM_TYPE_MBLA) { // MBLA
2694                 target_event=(quadlet_t *)(events + (stream->spec.position));
2695                
2696                 if(do_ringbuffer_read) {
2697                         read=freebob_ringbuffer_read(stream->buffer, (char *)buffer, nsamples*sizeof(freebob_sample_t))/sizeof(freebob_sample_t);
2698                 } else {
2699                         read=nsamples;
2700                 }
2701                
2702                 // TODO: convert this into function pointer based
2703                 switch(stream->buffer_type) {
2704                         default:
2705                         case freebob_buffer_type_uint24:
2706                                 for(j = 0; j < read; j += 1) { // decode max nsamples
2707                                         *target_event = htonl((*(buffer) & 0x00FFFFFF) | 0x40000000);
2708                                         buffer++;
2709                                         target_event+=dimension;
2710                                        
2711                 //                      fprintf(stderr,"[%03d, %02d: %08p %08X %08X]\n",j, stream->spec.position, target_event, *target_event, *buffer);
2712
2713                                 }
2714                                 break;
2715                         case freebob_buffer_type_float:
2716 /*                              for(j = 0; j < read; j += 1) { // decode max nsamples
2717                                         // don't care for overflow
2718                                         float v = *floatbuff * multiplier;  // v: -231 .. 231
2719                                         unsigned int tmp = ((int)v);
2720                                         *target_event = htonl((tmp >> 8) | 0x40000000);
2721                                        
2722                                         floatbuff++;
2723                                         target_event += connection->spec.dimension;
2724                                 }*/
2725                                 j=0;
2726                                 if(read>3) {
2727                                         for (j = 0; j < read-3; j += 4) {
2728                                                 asm("movups %[floatbuff], %%xmm0\n\t"
2729                                                                 "mulps %[ssemult], %%xmm0\n\t"
2730                                                                 "cvttps2pi %%xmm0, %[out1]\n\t"
2731                                                                 "movhlps %%xmm0, %%xmm0\n\t"
2732                                                                 "psrld $8, %[out1]\n\t"
2733                                                                 "cvttps2pi %%xmm0, %[out2]\n\t"
2734                                                                 "por %[mmxmask], %[out1]\n\t"
2735                                                                 "psrld $8, %[out2]\n\t"
2736                                                                 "por %[mmxmask], %[out2]\n\t"
2737                                                         : [out1] "=&y" (*(v2si*)&out[0]),
2738                                                 [out2] "=&y" (*(v2si*)&out[2])
2739                                                         : [floatbuff] "m" (*(v4sf*)floatbuff),
2740                                                 [ssemult] "x" (*(v4sf*)sse_multiplier),
2741                                                 [mmxmask] "y" (*(v2si*)sse_mask)
2742                                                         : "xmm0");
2743                                                 floatbuff += 4;
2744                                                 *target_event = htonl(out[0]);
2745                                                 target_event += dimension;
2746                                                 *target_event = htonl(out[1]);
2747                                                 target_event += dimension;
2748                                                 *target_event = htonl(out[2]);
2749                                                 target_event += dimension;
2750                                                 *target_event = htonl(out[3]);
2751                                                 target_event += dimension;
2752                                         }
2753                                 }
2754                                 for(; j < read; ++j) {
2755                                 // don't care for overflow
2756                                         float v = *floatbuff * multiplier;  // v: -231 .. 231
2757                                         unsigned int tmp = (int)v;
2758                                         *target_event = htonl((tmp >> 8) | 0x40000000);
2759                
2760                                         floatbuff++;
2761                                         target_event += dimension;
2762                                 }
2763
2764                                 asm volatile("emms");
2765                                 break;
2766                 }
2767                
2768                 /*             
2769                 for(j = 0; j < nsamples; j+=1) {
2770                         *target_event = htonl((*(buffer) & 0x00FFFFFF) | 0x40000000);
2771                         buffer++;
2772                         target_event+=connection->spec.dimension;
2773                 }
2774                 */
2775                
2776                 return read;
2777                
2778         } else if (stream->spec.format == IEC61883_STREAM_TYPE_MIDI) {
2779                 return nsamples; // for midi we always indicate a full read
2780        
2781         } else { //if (stream->spec.format == IEC61883_STREAM_TYPE_SPDIF) {
2782                 return nsamples; // for unsupported we always indicate a full read
2783        
2784         }
2785         return 0;
2786
2787 }
2788
2789 #else
2790
2791 static inline int 
2792 freebob_decode_events_to_stream(freebob_connection_t *connection,
2793                                                                 freebob_stream_t *stream,
2794                                                                 quadlet_t* events,
2795                                                                 unsigned int nsamples,
2796                                                                 unsigned int dbc
2797                                                                 ) {
2798         quadlet_t *target_event;
2799         int do_ringbuffer_write=0;
2800        
2801         assert (stream);
2802         assert (connection);
2803         assert (events);
2804         assert (stream->spec.position < connection->spec.dimension);
2805                
2806         freebob_sample_t *buffer=NULL;
2807         float *floatbuff=NULL;
2808
2809         const float multiplier = 1.0f / (float)(0x7FFFFF);
2810        
2811         int dimension=connection->spec.dimension;
2812        
2813         assert(stream->user_buffer);
2814        
2815         switch(stream->buffer_type) {
2816                 case freebob_buffer_type_per_stream:
2817                 default:
2818 //                      assert(nsamples < dev->options.period_size);
2819                        
2820                         // use the preallocated buffer (at init time)
2821                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2822                        
2823                         do_ringbuffer_write=1;
2824                         break;
2825                                
2826                 case freebob_buffer_type_uint24:
2827                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2828                        
2829                         do_ringbuffer_write=0;
2830                         break;
2831                                
2832                 case freebob_buffer_type_float:
2833                         floatbuff=((float *)(stream->user_buffer))+stream->user_buffer_position;
2834                        
2835                         do_ringbuffer_write=0;
2836                         break;
2837         }
2838        
2839         int j=0;
2840         int written=0;
2841        
2842         if (stream->spec.format== IEC61883_STREAM_TYPE_MBLA) {
2843                 target_event=(quadlet_t *)(events + stream->spec.position);
2844
2845                 // TODO: convert this into function pointer based
2846                 switch(stream->buffer_type) {
2847                         default:
2848                         case freebob_buffer_type_uint24:
2849                                 for(j = 0; j < nsamples; j += 1) { // decode max nsamples
2850                                         *(buffer)=(ntohl((*target_event) ) & 0x00FFFFFF);
2851                 //                      fprintf(stderr,"[%03d, %02d: %08p %08X %08X]\n",j, stream->spec.position, target_event, *target_event, *buffer);
2852                                         buffer++;
2853                                         target_event+=dimension;
2854                                 }
2855                                 break;
2856                         case freebob_buffer_type_float:
2857                                 for(j = 0; j < nsamples; j += 1) { // decode max nsamples               
2858
2859                                         unsigned int v = ntohl(*target_event) & 0x00FFFFFF;
2860                                         // sign-extend highest bit of 24-bit int
2861                                         int tmp = (int)(v << 8) / 256;
2862                
2863                                         *floatbuff = tmp * multiplier;
2864                                
2865                                         floatbuff++;
2866                                         target_event+=dimension;
2867                                 }
2868                                 break;
2869                 }
2870
2871 //              fprintf(stderr,"rb write [%02d: %08p %08p]\n",stream->spec.position, stream, stream->buffer);
2872        
2873 //      fprintf(stderr,"rb write [%02d: %08p %08p %08X, %d, %d]\n",stream->spec.position, stream, stream->buffer, *buffer, nsamples, written);
2874                 if(do_ringbuffer_write) {
2875                         // reset the buffer pointer
2876                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2877                         written=freebob_ringbuffer_write(stream->buffer, (char *)(buffer), nsamples*sizeof(freebob_sample_t))/sizeof(freebob_sample_t);
2878                 } else {
2879                         written=nsamples;
2880                 }
2881                
2882 //      fprintf(stderr,"rb write1[%02d: %08p %08p %08X, %d, %d]\n",stream->spec.position, stream, stream->buffer, *buffer, nsamples, written);
2883                
2884                 return written;
2885        
2886         } else if (stream->spec.format == IEC61883_STREAM_TYPE_MIDI) {
2887                 return nsamples; // for midi streams we always indicate a full write
2888        
2889         } else {//if (stream->spec.format == IEC61883_STREAM_TYPE_SPDIF) {
2890                 return nsamples; // for unsupported we always indicate a full read
2891        
2892         }
2893        
2894         return 0;
2895
2896
2897 }
2898
2899 static inline int
2900 freebob_encode_stream_to_events(freebob_connection_t *connection,
2901                                                                 freebob_stream_t *stream,
2902                                                                 quadlet_t* events,
2903                                                                 unsigned int nsamples,
2904                                                                 unsigned int dbc
2905                                                                 ) {
2906         quadlet_t *target_event;
2907         //freebob_sample_t buff[nsamples];
2908         int do_ringbuffer_read=0;
2909        
2910         freebob_sample_t *buffer=NULL;
2911         float *floatbuff=NULL;
2912
2913         const float multiplier = (float)(0x7FFFFF00);
2914         int dimension=connection->spec.dimension;
2915        
2916         unsigned int j=0;
2917         unsigned int read=0;
2918
2919         assert (stream);
2920         assert (connection);
2921         assert (events);
2922         assert (stream->spec.position < connection->spec.dimension);
2923        
2924         switch(stream->buffer_type) {
2925                 case freebob_buffer_type_per_stream:
2926                 default:
2927 //                      assert(nsamples < dev->options.period_size);
2928                         // use the preallocated buffer (at init time)
2929                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2930                        
2931                         do_ringbuffer_read=1;
2932                         break;
2933                                
2934                 case freebob_buffer_type_uint24:
2935                         buffer=((freebob_sample_t *)(stream->user_buffer))+stream->user_buffer_position;
2936                        
2937                         do_ringbuffer_read=0;
2938                         break;
2939                                
2940                 case freebob_buffer_type_float:
2941                         floatbuff=((float *)(stream->user_buffer))+stream->user_buffer_position;
2942                        
2943                         do_ringbuffer_read=0;
2944                         break;
2945         }
2946        
2947        
2948         if(stream->spec.format == IEC61883_STREAM_TYPE_MBLA) { // MBLA
2949                 target_event=(quadlet_t *)(events + (stream->spec.position));
2950                
2951                 if(do_ringbuffer_read) {
2952                         read=freebob_ringbuffer_read(stream->buffer, (char *)buffer, nsamples*sizeof(freebob_sample_t))/sizeof(freebob_sample_t);
2953                 } else {
2954                         read=nsamples;
2955                 }
2956                
2957                 // TODO: convert this into function pointer based
2958                 switch(stream->buffer_type) {
2959                         default:
2960                         case freebob_buffer_type_uint24:
2961                                 for(j = 0; j < read; j += 1) { // decode max nsamples
2962                                         *target_event = htonl((*(buffer) & 0x00FFFFFF) | 0x40000000);
2963                                         buffer++;
2964                                         target_event+=dimension;
2965                                        
2966                 //                      fprintf(stderr,"[%03d, %02d: %08p %08X %08X]\n",j, stream->spec.position, target_event, *target_event, *buffer);
2967
2968                                 }
2969                                 break;
2970                         case freebob_buffer_type_float:
2971                                 for(j = 0; j < read; j += 1) { // decode max nsamples
2972                                         // don't care for overflow
2973                                         float v = *floatbuff * multiplier;  // v: -231 .. 231
2974                                         unsigned int tmp = ((int)v);
2975                                         *target_event = htonl((tmp >> 8) | 0x40000000);
2976                                        
2977                                         floatbuff++;
2978                                         target_event += dimension;
2979                                 }
2980                                 break;
2981                 }
2982                
2983                 /*             
2984                 for(j = 0; j < nsamples; j+=1) {
2985                         *target_event = htonl((*(buffer) & 0x00FFFFFF) | 0x40000000);
2986                         buffer++;
2987                         target_event+=connection->spec.dimension;
2988                 }
2989                 */
2990                
2991                 return read;
2992                
2993         } else if (stream->spec.format == IEC61883_STREAM_TYPE_MIDI) {
2994                 return nsamples; // for midi we always indicate a full read
2995        
2996         } else { //if (stream->spec.format == IEC61883_STREAM_TYPE_SPDIF) {
2997                 return nsamples; // for unsupported we always indicate a full read
2998        
2999         }
3000         return 0;
3001
3002 }
3003 #endif
3004
3005 /*
3006  * write received events to the stream ringbuffers.
3007  */
3008
3009 int freebob_am824_recv(char *data,
3010                                            int nevents, unsigned int offset, unsigned int dbc,
3011                                freebob_connection_t *connection)
3012 {
3013         int xrun=0;
3014         unsigned int i=0;
3015        
3016         for(i = 0; i < connection->nb_streams; i++) {
3017                 freebob_stream_t *stream = &(connection->streams[i]);
3018                 size_t written=0;
3019                
3020                 assert(stream);
3021                
3022                 stream->user_buffer_position=offset;
3023                
3024                 debugPrintShort(DEBUG_LEVEL_HANDLERS,"[%d: %d ",i,freebob_ringbuffer_write_space(stream->buffer));
3025                 written = freebob_decode_events_to_stream(
3026                         connection,
3027                         stream,
3028                         (quadlet_t *)data,
3029                         nevents,
3030                         dbc
3031                         );
3032
3033                 if (written < nevents) {
3034                         xrun++;
3035                 }
3036                
3037                 debugPrintShort(DEBUG_LEVEL_HANDLERS, "%d %d]",written,freebob_ringbuffer_write_space(stream->buffer));
3038         }
3039        
3040 //      connection->status.frames_left-=nevents;
3041                
3042         debugPrintShort(DEBUG_LEVEL_HANDLERS, "\n");
3043        
3044         return xrun;
3045 }
3046
3047 /*
3048  * get xmit events from ringbuffer
3049  */
3050
3051 int freebob_am824_xmit( char *data,
3052                                                 int nevents, unsigned int offset, unsigned int dbc,
3053                                                 freebob_connection_t *connection)
3054 {
3055         int xrun=0;
3056         unsigned int i=0;
3057        
3058         for(i = 0; i < connection->nb_streams; i++) {
3059                 freebob_stream_t *stream = &(connection->streams[i]);
3060                 size_t read=0;
3061                
3062                 assert(stream);
3063                 debugPrintShort(DEBUG_LEVEL_HANDLERS, "[%d: %d ",i,freebob_ringbuffer_read_space(stream->buffer));
3064                
3065                 stream->user_buffer_position=offset;
3066                
3067                 read = freebob_encode_stream_to_events(
3068                         connection,
3069                         stream,
3070                         (quadlet_t *)data,
3071                         nevents,
3072                         dbc
3073                         );
3074                
3075                 debugPrintShort(DEBUG_LEVEL_HANDLERS, "%d %d]",read,freebob_ringbuffer_read_space(stream->buffer));
3076
3077                 if (read < nevents) {
3078                         xrun++;
3079                 }
3080                
3081         }
3082         debugPrintShort(DEBUG_LEVEL_HANDLERS, "\n");
3083        
3084 //      connection->status.frames_left-=nevents;
3085        
3086         return xrun;
3087 }
3088
3089 void *freebob_streaming_watchdog_thread (void *arg)
3090 {
3091         freebob_device_t *dev = (freebob_device_t *) arg;
3092
3093         dev->watchdog_check = 0;
3094
3095         while (1) {
3096                 sleep (2);
3097                 if (dev->watchdog_check == 0) {
3098
3099                         printError("watchdog: timeout");
3100
3101                         /* kill our process group, try to get a dump */
3102                         kill (-getpgrp(), SIGABRT);
3103                         /*NOTREACHED*/
3104                         exit (1);
3105                 }
3106                 dev->watchdog_check = 0;
3107         }
3108 }
3109
3110 int freebob_streaming_start_watchdog (freebob_device_t *dev)
3111 {
3112         int watchdog_priority = dev->packetizer.priority + 10;
3113         int max_priority = sched_get_priority_max (SCHED_FIFO);
3114
3115         debugPrint(DEBUG_LEVEL_STARTUP, "Starting Watchdog...\n");
3116        
3117         if ((max_priority != -1) &&
3118                         (max_priority < watchdog_priority))
3119                 watchdog_priority = max_priority;
3120        
3121         if (freebob_streaming_create_thread (dev, &dev->watchdog_thread, watchdog_priority,
3122                 TRUE, freebob_streaming_watchdog_thread, dev)) {
3123                         printError ("cannot start watchdog thread");
3124                         return -1;
3125                 }
3126
3127         return 0;
3128 }
3129
3130 void freebob_streaming_stop_watchdog (freebob_device_t *dev)
3131 {
3132         debugPrint(DEBUG_LEVEL_STARTUP, "Stopping Watchdog...\n");
3133        
3134         pthread_cancel (dev->watchdog_thread);
3135         pthread_join (dev->watchdog_thread, NULL);
3136 }
Note: See TracBrowser for help on using the browser.