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

Revision 194, 88.7 kB (checked in by pieterpalmers, 18 years ago)

- code cleanup in the streaming part.
- calculate ISO connection parameters instead of user-supplying them
- SSE code improvement

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