root/branches/libfreebob-2.0/src/libstreaming/MotuStreamProcessor.cpp

Revision 284, 38.4 kB (checked in by jwoithe, 16 years ago)

More MOTU iso transmission work. Things are now partially operational.

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,2006 Pieter Palmers <pieterpalmers@users.sourceforge.net>
10  *   Copyright (C) 2006 Jonathan Woithe <jwoithe@physics.adelaide.edu.au>
11  *
12  *   This program is free software {} you can redistribute it and/or modify
13  *   it under the terms of the GNU General Public License as published by
14  *   the Free Software Foundation {} either version 2 of the License, or
15  *   (at your option) any later version.
16  *
17  *   This program is distributed in the hope that it will be useful,
18  *   but WITHOUT ANY WARRANTY {} without even the implied warranty of
19  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
20  *   GNU General Public License for more details.
21  *
22  *   You should have received a copy of the GNU General Public License
23  *   along with this program {} if not, write to the Free Software
24  *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
25  *
26  *
27  *
28  */
29
30
31 #include "MotuStreamProcessor.h"
32 #include "Port.h"
33 #include "MotuPort.h"
34
35 #include <netinet/in.h>
36
37 namespace FreebobStreaming {
38
39 IMPL_DEBUG_MODULE( MotuTransmitStreamProcessor, MotuTransmitStreamProcessor, DEBUG_LEVEL_NORMAL );
40 IMPL_DEBUG_MODULE( MotuReceiveStreamProcessor, MotuReceiveStreamProcessor, DEBUG_LEVEL_NORMAL );
41
42 // A macro to extract specific bits from a native endian quadlet
43 #define get_bits(_d,_start,_len) (((_d)>>((_start)-(_len)+1)) & ((1<<(_len))-1))
44
45
46 /* transmit */
47 MotuTransmitStreamProcessor::MotuTransmitStreamProcessor(int port, int framerate,
48                 unsigned int event_size)
49         : TransmitStreamProcessor(port, framerate), m_event_size(event_size),
50         m_tx_dbc(0), m_cycle_count(-1), m_cycle_ofs(0.0), m_sph_ofs_dll(NULL) {
51
52 }
53
54 MotuTransmitStreamProcessor::~MotuTransmitStreamProcessor() {
55         freebob_ringbuffer_free(m_event_buffer);
56         free(m_tmp_event_buffer);
57 }
58
59 bool MotuTransmitStreamProcessor::init() {
60
61         debugOutput( DEBUG_LEVEL_VERBOSE, "Initializing (%p)...\n");
62         // call the parent init
63         // this has to be done before allocating the buffers,
64         // because this sets the buffersizes from the processormanager
65         if(!TransmitStreamProcessor::init()) {
66                 debugFatal("Could not do base class init (%p)\n",this);
67                 return false;
68         }
69        
70
71         return true;
72 }
73
74 void MotuTransmitStreamProcessor::setVerboseLevel(int l) {
75         setDebugLevel(l); // sets the debug level of the current object
76         TransmitStreamProcessor::setVerboseLevel(l); // also set the level of the base class
77 }
78
79
80 enum raw1394_iso_disposition
81 MotuTransmitStreamProcessor::getPacket(unsigned char *data, unsigned int *length,
82                       unsigned char *tag, unsigned char *sy,
83                       int cycle, unsigned int dropped, unsigned int max_length) {
84
85 // FIXME: the actual delays in the system need to be worked out so
86 // we can get this thing synchronised.  For now this seems to work.
87 #define CYCLE_DELAY 1
88
89 // FIXME: currently data is not sent when the stream is disabled.  The
90 // trouble is that the MOTU actually needs zero data explicitly sent from
91 // the moment its iso receive channel is activated; failure to do so can
92 // result in a high pitch audio signal (approx 10 kHz) in channels which
93 // have had non-zero data in the past.  Things need to be changed around so
94 // this can be done; essentially the tests on m_disabled disappear from
95 // almost everywhere.  Instead, m_disabled will determine whether data is
96 // fetched from the event buffer or whether zero data is generated.
97 //
98 // The other thing which needs to be worked out is close-down.
99 // Experimentation has shown that 2 or so zero packets need to be sent so no
100 // high-pitched noises are emitted at closedown and subsequent restart.  In
101 // the proof-of-concept code this was done by manually calling
102 // raw1394_loop_iterate() from the iso shutdown function.  Under freebob a
103 // similar thing needs to be done from the respective function in the Motu
104 // AvDevice object, but the precise way of doing so without causing issues
105 // is yet to be determined.
106 //
107 // Finally, every so often sync seems to be missed on startup, and because
108 // this code can't recover a sync the problem remains indefinitely.  The
109 // cause of this needs to be identified.  It may be the result of not
110 // running with RT privileges for this initial testing phase.
111
112         enum raw1394_iso_disposition retval = RAW1394_ISO_OK;
113         quadlet_t *quadlet = (quadlet_t *)data;
114         signed int i;
115
116         // signal that we are running
117         // this is to allow the manager to wait untill all streams are up&running
118         // it can take some time before the devices start to transmit.
119         // if we would transmit ourselves, we'd have instant buffer underrun
120         // this works in cooperation with the m_disabled value
121        
122         // TODO: add code here to detect that a stream is running
123         // NOTE: xmit streams are most likely 'always' ready
124         m_running=true;
125        
126         // Initialise the cycle counter if this is the first time
127         // iso data has been requested with the stream enabled.
128         if (!m_disabled && m_cycle_count<0) {
129                 m_cycle_count = cycle;
130                 m_cycle_ofs = 0.0;
131         }
132
133         // Do housekeeping expected for all packets sent to the MOTU, even
134         // for packets containing no audio data.
135         *sy = 0x00;
136         *tag = 1;      // All MOTU packets have a CIP-like header
137
138         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "get packet...\n");
139
140         // Size of a single data frame in quadlets
141         unsigned dbs = m_event_size / 4;
142
143         // The number of events expected by the MOTU is solely dependent on
144         // the current sample rate.  An 'event' is one sample from all channels
145         // plus possibly other midi and control data.
146         signed n_events = m_framerate<=48000?8:(m_framerate<=96000?16:32);
147
148         // Increment the dbc (data block count).  This is only done if the
149         // packet will contain events - that is, the stream is not disabled
150         // and we are due to send some data.  Otherwise a pad packet is sent
151         // which contains the DBC of the previously sent packet.  This
152         // regime also means that the very first packet containing data will
153         // have a DBC of n_events, which matches what is observed from other
154         // systems.
155         if (!m_disabled && cycle>=m_cycle_count) {
156                 m_tx_dbc += n_events;
157                 if (m_tx_dbc > 0xff)
158                         m_tx_dbc -= 0x100;
159         }
160
161         // construct the packet CIP-like header.  Even if this is a data-less
162         // packet the dbs field is still set as if there were data blocks
163         // present.  For data-less packets the dbc is the same as the previously
164         // transmitted block.
165         *quadlet = htonl(0x00000400 | ((getNodeId()&0x3f)<<24) | m_tx_dbc | (dbs<<16));
166         quadlet++;
167         *quadlet = htonl(0x8222ffff);
168         quadlet++;
169         *length = 8;
170
171         // If the stream is disabled or the MOTU transmission cycle count is
172         // ahead of the ieee1394 cycle timer, we send a data-less packet
173         // with only the 8 byte CIP-like header set up previously.
174         // FIXME: in disabled state, need to send a stream of zero audio data,
175         // not "no data".  Otherwise MOTU will emit an (approx) 10 kHz signal.
176         if (m_disabled || cycle<m_cycle_count) {
177                 return RAW1394_ISO_OK;
178         }
179
180         // Size of data to read from the event buffer, in bytes.
181         unsigned int read_size = n_events * m_event_size;
182
183         // We read the packet data from a ringbuffer because of efficiency;
184         // it allows us to construct the packets one period at once.
185         if ((freebob_ringbuffer_read(m_event_buffer,(char *)(data+8),read_size)) <
186                                 read_size) {
187                 /* there is no more data in the ringbuffer */
188                 debugWarning("Transmit buffer underrun (cycle %d, FC=%d, PC=%d)\n",
189                         cycle, m_framecounter, m_handler->getPacketCount());
190
191                 // signal underrun
192                 m_xruns++;
193
194                 retval=RAW1394_ISO_DEFER; // make raw1394_loop_iterate exit its inner loop
195                 n_events = 0;
196
197         } else {
198                 retval=RAW1394_ISO_OK;
199                 *length += read_size;
200
201                 // Set up each frames's SPH.  Note that the (int) typecast
202                 // appears to do rounding.
203                 // FIXME: once working, make more efficient by removing 1 of the
204                 // "trim to 8000" operations.
205                 for (i=0; i<n_events; i++, quadlet += dbs) {
206                         *quadlet = htonl( (((m_cycle_count+CYCLE_DELAY)%8000)<<12) +
207                                         (int)m_cycle_ofs);
208 // FIXME: remove this hacked in 1 kHz test signal to analog-1
209 {
210 signed int val;
211 val = 0x7fffff*sin(1000.0*2.0*M_PI*(m_cycle_count+((m_cycle_ofs)/3072.0))/8000.0);
212 *(data+8+i*m_event_size+16) = (val >> 16) & 0xff;
213 *(data+8+i*m_event_size+17) = (val >> 8) & 0xff;
214 *(data+8+i*m_event_size+18) = val & 0xff;
215 }
216                         m_cycle_ofs += m_sph_ofs_dll->get();
217                         if (m_cycle_ofs >= 3072) {
218                                 m_cycle_ofs -= 3072;
219                                 if (++m_cycle_count > 7999)
220                                         m_cycle_count -= 8000;
221                         }
222                 }
223 #if 0
224 //if (cycle==10) {
225 if (m_cycle_count==7999 || m_cycle_count==0) {
226 int j;
227   for (j=0; j<n_events; j++) {
228     for (i=0; i<25; i++)
229       fprintf(stderr,"%02hhx ",*(data+8+j*dbs*4+i));
230     fprintf(stderr,"\n");
231   }
232 }
233 #endif
234                 // Process all ports that should be handled on a per-packet base
235                 // this is MIDI for AMDTP (due to the need of DBC, which is lost
236                 // when putting the events in the ringbuffer)
237                 // for motu this might also be control data, however as control
238                 // data isn't time specific I would also include it in the period
239                 // based processing
240        
241                 // FIXME: m_tx_dbc probably needs to be initialised to a non-zero
242                 // value somehow so MIDI sync is possible.  For now we ignore
243                 // this issue.
244                 if (!encodePacketPorts((quadlet_t *)(data+8), n_events, m_tx_dbc)) {
245                         debugWarning("Problem encoding Packet Ports\n");
246                 }
247         }
248    
249         // Update the frame counter
250         incrementFrameCounter(n_events);
251
252         // Keep this at the end, because otherwise the raw1394_loop_iterate
253         // functions inner loop keeps requesting packets, that are not
254         // nescessarily ready
255
256 // Amdtp has this commented out
257         if (m_framecounter > (signed int)m_period) {
258                 retval=RAW1394_ISO_DEFER;
259         }
260        
261         return retval;
262 }
263
264 bool MotuTransmitStreamProcessor::isOnePeriodReady() {
265         // TODO: this is the way you can implement sync
266         //       only when this returns true, one period will be
267         //       transferred to the audio api side.
268         //       you can delay this moment as long as you
269         //       want (provided that there is enough buffer space)
270        
271         // this implementation just waits until there is one period of samples
272         // transmitted from the buffer
273
274 // Amdtp has this commented out and simply return true.
275         return (m_framecounter > (signed int)m_period);
276 //      return true;
277 }
278  
279 bool MotuTransmitStreamProcessor::prefill() {
280         // this is needed because otherwise there is no data to be
281         // sent when the streaming starts
282    
283         int i = m_nb_buffers;
284         while (i--) {
285                 if(!transferSilence(m_period)) {
286                         debugFatal("Could not prefill transmit stream\n");
287                         return false;
288                 }
289         }
290         return true;
291 }
292
293 bool MotuTransmitStreamProcessor::reset() {
294
295         debugOutput( DEBUG_LEVEL_VERBOSE, "Resetting...\n");
296
297         // reset the event buffer, discard all content
298         freebob_ringbuffer_reset(m_event_buffer);
299    
300         // reset all non-device specific stuff
301         // i.e. the iso stream and the associated ports
302         if (!TransmitStreamProcessor::reset()) {
303                 debugFatal("Could not do base class reset\n");
304                 return false;
305         }
306
307         // we should prefill the event buffer
308         if (!prefill()) {
309                 debugFatal("Could not prefill buffers\n");
310                 return false;   
311         }
312
313         return true;
314 }
315
316 bool MotuTransmitStreamProcessor::prepare() {
317    
318         debugOutput( DEBUG_LEVEL_VERBOSE, "Preparing...\n");
319    
320         // prepare all non-device specific stuff
321         // i.e. the iso stream and the associated ports
322         if (!TransmitStreamProcessor::prepare()) {
323                 debugFatal("Could not prepare base class\n");
324                 return false;
325         }
326
327         m_PeriodStat.setName("XMT PERIOD");
328         m_PacketStat.setName("XMT PACKET");
329         m_WakeupStat.setName("XMT WAKEUP");
330
331         debugOutput( DEBUG_LEVEL_VERBOSE, "Event size: %d\n", m_event_size);
332    
333         // allocate the event buffer
334         unsigned int ringbuffer_size_frames=m_nb_buffers * m_period;
335    
336         if( !(m_event_buffer=freebob_ringbuffer_create(
337           m_event_size * ringbuffer_size_frames))) {
338                 debugFatal("Could not allocate memory event ringbuffer");
339                 return false;
340         }
341
342         // Allocate the temporary event buffer.  This is needed for the
343         // efficient transfer() routine.  Its size has to be equal to one
344         // 'event'.
345         if( !(m_tmp_event_buffer=(char *)calloc(1,m_event_size))) {
346                 debugFatal("Could not allocate temporary event buffer");
347                 freebob_ringbuffer_free(m_event_buffer);
348                 return false;
349         }
350
351         // Set the parameters of ports we can: we want the audio ports to be
352         // period buffered, and the midi ports to be packet buffered.
353         for ( PortVectorIterator it = m_Ports.begin();
354           it != m_Ports.end();
355           ++it ) {
356                 debugOutput(DEBUG_LEVEL_VERBOSE, "Setting up port %s\n",(*it)->getName().c_str());
357                 if(!(*it)->setBufferSize(m_period)) {
358                         debugFatal("Could not set buffer size to %d\n",m_period);
359                         return false;
360                 }
361
362                 switch ((*it)->getPortType()) {
363                 case Port::E_Audio:
364                         if (!(*it)->setSignalType(Port::E_PeriodSignalled)) {
365                                 debugFatal("Could not set signal type to PeriodSignalling");
366                                 return false;
367                         }
368                         break;
369
370                 case Port::E_Midi:
371                         if (!(*it)->setSignalType(Port::E_PacketSignalled)) {
372                                 debugFatal("Could not set signal type to PacketSignalling");
373                                 return false;
374                         }
375                         break;
376                
377                 case Port::E_Control:
378                         if (!(*it)->setSignalType(Port::E_PeriodSignalled)) {
379                                 debugFatal("Could not set signal type to PeriodSignalling");
380                                 return false;
381                         }
382                         break;
383
384                 default:
385                         debugWarning("Unsupported port type specified\n");
386                         break;
387                 }
388         }
389
390         // The API specific settings of the ports are already set before
391         // this routine is called, therefore we can init&prepare the ports
392         if (!initPorts()) {
393                 debugFatal("Could not initialize ports!\n");
394                 return false;
395         }
396
397         if(!preparePorts()) {
398                 debugFatal("Could not initialize ports!\n");
399                 return false;
400         }
401
402         // We should prefill the event buffer
403         if (!prefill()) {
404                 debugFatal("Could not prefill buffers\n");
405                 return false;   
406         }
407
408         return true;
409 }
410
411 bool MotuTransmitStreamProcessor::transferSilence(unsigned int size) {
412    
413         // This function should tranfer 'size' frames of 'silence' to the event buffer
414         unsigned int write_size=size*m_event_size;
415         char *dummybuffer=(char *)calloc(size,m_event_size);
416
417         transmitSilenceBlock(dummybuffer, size, 0);
418
419         if (freebob_ringbuffer_write(m_event_buffer,(char *)(dummybuffer),write_size) < write_size) {
420                 debugWarning("Could not write to event buffer\n");
421         }
422
423         free(dummybuffer);
424
425         return true;
426 }
427
428 /**
429  * \brief write events queued for transmission from the port ringbuffers
430  * to the event buffer.
431  */
432 bool MotuTransmitStreamProcessor::transfer() {
433         m_PeriodStat.mark(freebob_ringbuffer_read_space(m_event_buffer)/m_event_size);
434
435         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "Transferring period...\n");
436         // TODO: improve
437 /* a naive implementation would look like this:
438
439         unsigned int write_size=m_period*m_event_size;
440         char *dummybuffer=(char *)calloc(m_period,m_event_size);
441
442         transmitBlock(dummybuffer, m_period, 0, 0);
443
444         if (freebob_ringbuffer_write(m_event_buffer,(char *)(dummybuffer),write_size) < write_size) {
445                 debugWarning("Could not write to event buffer\n");
446         }
447
448         free(dummybuffer);
449 */
450 /* but we're not that naive anymore... */
451         int xrun;
452         unsigned int offset=0;
453
454 // FIXME: just return until we've got the transmit side of things functional
455 //return true;
456
457         freebob_ringbuffer_data_t vec[2];
458         // There is one period of frames to transfer.  This is
459         // period_size*m_event_size of events.
460         unsigned int bytes2write=m_period*m_event_size;
461
462         /* Write bytes2write bytes to the event ringbuffer.  First see if it can
463          * be done in one write; if so, ok.
464          * Otherwise write up to a multiple of events directly to the buffer
465          * then do the buffer wrap around using ringbuffer_write.  Then
466          * write the remaining data directly to the buffer in a third pass.
467          * Make sure that we cannot end up on a non-cluster aligned
468          * position!
469          */
470         while(bytes2write>0) {
471                 int byteswritten=0;
472        
473                 unsigned int frameswritten=(m_period*m_event_size-bytes2write)/m_event_size;
474                 offset=frameswritten;
475
476                 freebob_ringbuffer_get_write_vector(m_event_buffer, vec);
477
478                 if (vec[0].len==0) { // this indicates a full event buffer
479                         debugError("XMT: Event buffer overrun in processor %p\n",this);
480                         break;
481                 }
482
483                 /* If we don't take care we will get stuck in an infinite
484                  * loop because we align to a event boundary later.  The
485                  * remaining nb of bytes in one write operation can be
486                  * smaller than one event; this can happen because the
487                  * ringbuffer size is always a power of 2.
488                  */
489                 if(vec[0].len<m_event_size) {
490            
491                         // encode to the temporary buffer
492                         xrun = transmitBlock(m_tmp_event_buffer, 1, offset);
493            
494                         if (xrun<0) {
495                                 // xrun detected
496                                 debugError("XMT: Frame buffer underrun in processor %p\n",this);
497                                 break;
498                         }
499
500                         // Use the ringbuffer function to write one event.
501                         // The write function handles the wrap around.
502                         freebob_ringbuffer_write(m_event_buffer,
503                                 m_tmp_event_buffer, m_event_size);
504                
505                         // we advanced one m_event_size
506                         bytes2write-=m_event_size;
507                
508                 } else {
509            
510                         if (bytes2write>vec[0].len) {
511                                 // align to an event boundary
512                                 byteswritten=vec[0].len-(vec[0].len%m_event_size);
513                         } else {
514                                 byteswritten=bytes2write;
515                         }
516
517                         xrun = transmitBlock(vec[0].buf,
518                                 byteswritten/m_event_size, offset);
519            
520                         if (xrun<0) {
521                                 // xrun detected
522                                 debugError("XMT: Frame buffer underrun in processor %p\n",this);
523                                 break;
524                         }
525
526                         freebob_ringbuffer_write_advance(m_event_buffer, byteswritten);
527                         bytes2write -= byteswritten;
528                 }
529
530                 // the bytes2write should always be event aligned
531                 assert(bytes2write%m_event_size==0);
532         }
533
534         return true;
535 }
536 /*
537  * write received events to the stream ringbuffers.
538  */
539
540 int MotuTransmitStreamProcessor::transmitBlock(char *data,
541                        unsigned int nevents, unsigned int offset) {
542         signed int problem=0;
543         unsigned int i;
544
545         // FIXME: ensure the MIDI and control streams are all zeroed until
546         // such time as they are fully implemented.
547         for (i=0; i<nevents; i++) {
548                 memset(data+4+i*m_event_size, 0x00, 6);
549         }
550
551         for ( PortVectorIterator it = m_PeriodPorts.begin();
552           it != m_PeriodPorts.end();
553           ++it ) {
554                 // If this port is disabled, don't process it
555                 if((*it)->isDisabled()) {continue;};
556        
557                 //FIXME: make this into a static_cast when not DEBUG?
558                 Port *port=dynamic_cast<Port *>(*it);
559                
560                 switch(port->getPortType()) {
561                
562                 case Port::E_Audio:
563                         if (encodePortToMBLAEvents(static_cast<MotuAudioPort *>(*it), (quadlet_t *)data, offset, nevents)) {
564                                 debugWarning("Could not encode port %s to MBLA events",(*it)->getName().c_str());
565                                 problem=1;
566                         }
567                         break;
568                 // midi is a packet based port, don't process
569                 //      case MotuPortInfo::E_Midi:
570                 //              break;
571
572                 default: // ignore
573                         break;
574                 }
575         }
576         return problem;
577 }
578
579 int MotuTransmitStreamProcessor::transmitSilenceBlock(char *data,
580                        unsigned int nevents, unsigned int offset) {
581         // This is the same as the non-silence version, except that is
582         // doesn't read from the port buffers.
583
584         int problem=0;
585
586         for ( PortVectorIterator it = m_PeriodPorts.begin();
587           it != m_PeriodPorts.end();
588           ++it ) {
589                 //FIXME: make this into a static_cast when not DEBUG?
590                 Port *port=dynamic_cast<Port *>(*it);
591                
592                 switch(port->getPortType()) {
593                
594                 case Port::E_Audio:
595                         if (encodeSilencePortToMBLAEvents(static_cast<MotuAudioPort *>(*it), (quadlet_t *)data, offset, nevents)) {
596                                 debugWarning("Could not encode port %s to MBLA events",(*it)->getName().c_str());
597                                 problem=1;
598                         }
599                         break;
600                 // midi is a packet based port, don't process
601                 //      case MotuPortInfo::E_Midi:
602                 //              break;
603
604                 default: // ignore
605                         break;
606                 }
607         }
608         return problem;
609 }
610
611 /**
612  * @brief decode a packet for the packet-based ports
613  *
614  * @param data Packet data
615  * @param nevents number of events in data (including events of other ports & port types)
616  * @param dbc DataBlockCount value for this packet
617  * @return true if all successfull
618  */
619 bool MotuTransmitStreamProcessor::encodePacketPorts(quadlet_t *data, unsigned int nevents, unsigned int dbc)
620 {
621     bool ok=true;
622     char byte;
623    
624     quadlet_t *target_event=NULL;
625     int j;
626
627     for ( PortVectorIterator it = m_PacketPorts.begin();
628           it != m_PacketPorts.end();
629           ++it )
630     {
631
632 #ifdef DEBUG
633         MotuPortInfo *pinfo=dynamic_cast<MotuPortInfo *>(*it);
634         assert(pinfo); // this should not fail!!
635
636         // the only packet type of events for AMDTP is MIDI in mbla
637 //         assert(pinfo->getFormat()==MotuPortInfo::E_Midi);
638 #endif
639        
640         MotuMidiPort *mp=static_cast<MotuMidiPort *>(*it);
641        
642         // TODO: decode the midi (or other type) stuff here
643
644     }
645        
646     return ok;
647 }
648
649 /* Left in as reference, this is highly AMDTP related
650
651 basic idea:
652
653 iterate over the ports
654 - get port buffer address
655 - loop over events
656   * pick right sample in event based upon PortInfo
657   * convert sample from Port format (E_Int24, E_Float, ..) to native format
658
659 not that in order to use the 'efficient' transfer method, you have to make sure that
660 you can start from an offset (expressed in frames).
661 */
662 #include <math.h>
663
664 int MotuTransmitStreamProcessor::encodePortToMBLAEvents(MotuAudioPort *p, quadlet_t *data,
665                        unsigned int offset, unsigned int nevents) {
666         unsigned int j=0;
667         unsigned char *target = (unsigned char *)data + p->getPosition();
668
669 // offset is offset into the port buffers
670 //quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
671 //assert(nevents + offset <= p->getBufferSize());
672 //buffer+=offset;
673
674
675 // FIXME: use the form of the silence version here for testing
676         switch (p->getDataType()) {
677         default:
678         case Port::E_Int24:
679         case Port::E_Float:
680 // send silence to all outputs for now
681                 for (j = 0; j < nevents; j++) {
682 signed int val = 0;
683   val = 0;
684                         *target = (val >> 16) & 0xff;
685                         *(target+1) = (val >> 8) & 0xff;
686                         *(target+2) = val & 0xff;
687                         target += m_event_size;
688                 }
689                 break;
690         }
691
692
693 /*
694     quadlet_t *target_event;
695
696     target_event=(quadlet_t *)(data + p->getPosition());
697
698     switch(p->getDataType()) {
699         default:
700         case Port::E_Int24:
701             {
702                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
703
704                 assert(nevents + offset <= p->getBufferSize());
705
706                 buffer+=offset;
707
708                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
709                     *target_event = htonl((*(buffer) & 0x00FFFFFF) | 0x40000000);
710                     buffer++;
711                     target_event += m_dimension;
712                 }
713             }
714             break;
715         case Port::E_Float:
716             {
717                 const float multiplier = (float)(0x7FFFFF00);
718                 float *buffer=(float *)(p->getBufferAddress());
719
720                 assert(nevents + offset <= p->getBufferSize());
721
722                 buffer+=offset;
723
724                 for(j = 0; j < nevents; j += 1) { // decode max nsamples               
725    
726                     // don't care for overflow
727                     float v = *buffer * multiplier;  // v: -231 .. 231
728                     unsigned int tmp = ((int)v);
729                     *target_event = htonl((tmp >> 8) | 0x40000000);
730                    
731                     buffer++;
732                     target_event += m_dimension;
733                 }
734             }
735             break;
736     }
737 */
738         return 0;
739 }
740
741 int MotuTransmitStreamProcessor::encodeSilencePortToMBLAEvents(MotuAudioPort *p, quadlet_t *data,
742                        unsigned int offset, unsigned int nevents) {
743         unsigned int j=0;
744         unsigned char *target = (unsigned char *)data + p->getPosition();
745
746         switch (p->getDataType()) {
747         default:
748         case Port::E_Int24:
749         case Port::E_Float:
750                 for (j = 0; j < nevents; j++) {
751                         *target = *(target+1) = *(target+2) = 0;
752                         target += m_event_size;
753                 }
754                 break;
755         }
756
757         return 0;
758 }
759
760 /* --------------------- RECEIVE ----------------------- */
761
762 MotuReceiveStreamProcessor::MotuReceiveStreamProcessor(int port, int framerate,
763         unsigned int event_size)
764     : ReceiveStreamProcessor(port, framerate), m_event_size(event_size),
765         m_last_cycle_ofs(-1) {
766
767         // Set up the Delay-locked-loop to track audio frequency relative
768         // to the cycle timer.  The seed value is just the difference one
769         // would see if the audio clock was locked to the ieee1394 cycle
770         // timer.
771         // FIXME: the value for omega and coeff[0] are more or less copied
772         // from the test-dll.cpp code.  They need to be understood and
773         // optimised for this process.
774         float omega=6.28*0.001;
775         float coeffs[1];
776         coeffs[0]=1.41*omega;
777         m_sph_ofs_dll = new FreebobUtil::DelayLockedLoop(1, coeffs);
778         m_sph_ofs_dll->setIntegrator(0, 24576000.0/framerate);
779 }
780
781 MotuReceiveStreamProcessor::~MotuReceiveStreamProcessor() {
782         freebob_ringbuffer_free(m_event_buffer);
783         free(m_tmp_event_buffer);
784         delete m_sph_ofs_dll;
785 }
786
787 bool MotuReceiveStreamProcessor::init() {
788
789     // call the parent init
790     // this has to be done before allocating the buffers,
791     // because this sets the buffersizes from the processormanager
792     if(!ReceiveStreamProcessor::init()) {
793         debugFatal("Could not do base class init (%d)\n",this);
794         return false;
795     }
796
797     return true;
798 }
799
800 enum raw1394_iso_disposition
801 MotuReceiveStreamProcessor::putPacket(unsigned char *data, unsigned int length,
802                   unsigned char channel, unsigned char tag, unsigned char sy,
803                   unsigned int cycle, unsigned int dropped) {
804    
805     enum raw1394_iso_disposition retval=RAW1394_ISO_OK;
806
807 // FIXME: just for debugging, print out the sph ofs DLL value
808 // once a second
809 //if (cycle==0) {
810 //  fprintf(stderr, "sph_ofs_dll=%g\n",m_sph_ofs_dll->get());
811 //}
812
813     // If the packet length is 8 bytes (ie: just a CIP-like header) there is
814     // no isodata.
815     if (length > 8) {
816         // The iso data blocks from the MOTUs comprise a CIP-like header
817         // followed by a number of events (8 for 1x rates, 16 for 2x rates,
818         // 32 for 4x rates).
819         quadlet_t *quadlet = (quadlet_t *)data;
820         unsigned int dbs = get_bits(ntohl(quadlet[0]), 23, 8);  // Size of one event in terms of fdf_size
821         unsigned int fdf_size = get_bits(ntohl(quadlet[1]), 23, 8) == 0x22 ? 32:0; // Event unit size in bits
822         unsigned int event_length = (fdf_size * dbs) / 8;       // Event size in bytes
823         unsigned int n_events = (length-8) / event_length;
824
825         // Don't even attempt to process a packet if it isn't what we expect
826         // from a MOTU
827         if (tag!=1 || fdf_size!=32) {
828                 return RAW1394_ISO_OK;
829         }
830
831         // Signal that we're running
832         if (n_events) m_running=true;
833
834         // Don't process the stream when it is not enabled.
835         if (m_disabled) {
836                 return RAW1394_ISO_OK;
837         }
838        
839         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "put packet...\n");
840
841         // Add the data payload (events) to the ringbuffer.  We'll just copy
842         // everything including the 4 byte timestamp at the start of each
843         // event (that is, everything except the CIP-like header).  The
844         // demultiplexer can deal with the complexities such as the channel
845         // 24-bit data.
846         unsigned int write_size = length-8;
847         if (freebob_ringbuffer_write(m_event_buffer,(char *)(data+8),write_size) < write_size) {
848                 debugWarning("Receive buffer overrun (cycle %d, FC=%d, PC=%d)\n",
849                         cycle, m_framecounter, m_handler->getPacketCount());
850                 m_xruns++;
851
852                 retval=RAW1394_ISO_DEFER;
853         } else {
854                 retval=RAW1394_ISO_OK;
855                 // Process all ports that should be handled on a per-packet basis
856                 // This is MIDI for AMDTP (due to the need of DBC)
857                 int dbc = get_bits(ntohl(quadlet[0]), 8, 8);  // Low byte of CIP quadlet 0
858                 if (!decodePacketPorts((quadlet_t *)(data+8), n_events, dbc)) {
859                         debugWarning("Problem decoding Packet Ports\n");
860                         retval=RAW1394_ISO_DEFER;
861                 }
862                 // time stamp processing can be done here
863         }
864
865         // update the frame counter
866         incrementFrameCounter(n_events);
867         // keep this at the end, because otherwise the raw1394_loop_iterate functions inner loop
868         // keeps requesting packets without going to the xmit handler, leading to xmit starvation
869         if(m_framecounter>(signed int)m_period) {
870                 retval=RAW1394_ISO_DEFER;
871         }
872        
873     } else { // no events in packet
874         // discard packet
875         // can be important for sync though
876     }
877    
878     return retval;
879 }
880
881 bool MotuReceiveStreamProcessor::isOnePeriodReady() {
882      // TODO: this is the way you can implement sync
883      //       only when this returns true, one period will be
884      //       transferred to the audio api side.
885      //       you can delay this moment as long as you
886      //       want (provided that there is enough buffer space)
887      
888      // this implementation just waits until there is one period of samples
889      // received into the buffer
890     if(m_framecounter > (signed int)m_period) {
891         return true;
892     }
893     return false;
894 }
895
896 void MotuReceiveStreamProcessor::setVerboseLevel(int l) {
897         setDebugLevel(l);
898         ReceiveStreamProcessor::setVerboseLevel(l);
899
900 }
901
902
903 bool MotuReceiveStreamProcessor::reset() {
904
905         debugOutput( DEBUG_LEVEL_VERBOSE, "Resetting...\n");
906
907         // reset the event buffer, discard all content
908         freebob_ringbuffer_reset(m_event_buffer);
909
910         // reset all non-device specific stuff
911         // i.e. the iso stream and the associated ports
912         if(!ReceiveStreamProcessor::reset()) {
913                 debugFatal("Could not do base class reset\n");
914                 return false;
915         }
916        
917         return true;
918 }
919
920 bool MotuReceiveStreamProcessor::prepare() {
921
922         // prepare all non-device specific stuff
923         // i.e. the iso stream and the associated ports
924         if(!ReceiveStreamProcessor::prepare()) {
925                 debugFatal("Could not prepare base class\n");
926                 return false;
927         }
928
929         debugOutput( DEBUG_LEVEL_VERBOSE, "Preparing...\n");
930
931         m_PeriodStat.setName("RCV PERIOD");
932         m_PacketStat.setName("RCV PACKET");
933         m_WakeupStat.setName("RCV WAKEUP");
934
935     // setup any specific stuff here
936
937         debugOutput( DEBUG_LEVEL_VERBOSE, "Event size: %d\n", m_event_size);
938    
939         // allocate the event buffer
940         unsigned int ringbuffer_size_frames=m_nb_buffers * m_period;
941
942         if( !(m_event_buffer=freebob_ringbuffer_create(
943                         m_event_size * ringbuffer_size_frames))) {
944                 debugFatal("Could not allocate memory event ringbuffer");
945                 return false;
946         }
947
948         // allocate the temporary event buffer
949         if( !(m_tmp_event_buffer=(char *)calloc(1,m_event_size))) {
950                 debugFatal("Could not allocate temporary event buffer");
951                 freebob_ringbuffer_free(m_event_buffer);
952                 return false;
953         }
954
955         // set the parameters of ports we can:
956         // we want the audio ports to be period buffered,
957         // and the midi ports to be packet buffered
958         for ( PortVectorIterator it = m_Ports.begin();
959                   it != m_Ports.end();
960                   ++it )
961         {
962                 debugOutput(DEBUG_LEVEL_VERBOSE, "Setting up port %s\n",(*it)->getName().c_str());
963                
964                 if(!(*it)->setBufferSize(m_period)) {
965                         debugFatal("Could not set buffer size to %d\n",m_period);
966                         return false;
967                 }
968
969                 switch ((*it)->getPortType()) {
970                         case Port::E_Audio:
971                                 if(!(*it)->setSignalType(Port::E_PeriodSignalled)) {
972                                         debugFatal("Could not set signal type to PeriodSignalling");
973                                         return false;
974                                 }
975                                 break;
976                         case Port::E_Midi:
977                                 if(!(*it)->setSignalType(Port::E_PacketSignalled)) {
978                                         debugFatal("Could not set signal type to PacketSignalling");
979                                         return false;
980                                 }
981                                 break;
982                         case Port::E_Control:
983                                 if(!(*it)->setSignalType(Port::E_PeriodSignalled)) {
984                                         debugFatal("Could not set signal type to PeriodSignalling");
985                                         return false;
986                                 }
987                                 break;
988                         default:
989                                 debugWarning("Unsupported port type specified\n");
990                                 break;
991                 }
992
993         }
994
995         // The API specific settings of the ports are already set before
996         // this routine is called, therefore we can init&prepare the ports
997         if(!initPorts()) {
998                 debugFatal("Could not initialize ports!\n");
999                 return false;
1000         }
1001
1002         if(!preparePorts()) {
1003                 debugFatal("Could not initialize ports!\n");
1004                 return false;
1005         }
1006        
1007         return true;
1008
1009 }
1010
1011 bool MotuReceiveStreamProcessor::transfer() {
1012
1013     // the same idea as the transmit processor
1014    
1015         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "Transferring period...\n");
1016        
1017 /* another naive section:       
1018         unsigned int read_size=m_period*m_event_size;
1019         char *dummybuffer=(char *)calloc(m_period,m_event_size);
1020         if (freebob_ringbuffer_read(m_event_buffer,(char *)(dummybuffer),read_size) < read_size) {
1021                 debugWarning("Could not read from event buffer\n");
1022         }
1023
1024         receiveBlock(dummybuffer, m_period, 0);
1025
1026         free(dummybuffer);
1027 */
1028         int xrun;
1029         unsigned int offset=0;
1030        
1031         freebob_ringbuffer_data_t vec[2];
1032         // We received one period of frames from each channel.
1033         // This is period_size*m_event_size bytes.
1034         unsigned int bytes2read = m_period * m_event_size;
1035
1036         /* Read events2read bytes from the ringbuffer.
1037         *  First see if it can be done in one read.  If so, ok.
1038         *  Otherwise read up to a multiple of events directly from the buffer
1039         *  then do the buffer wrap around using ringbuffer_read
1040         *  then read the remaining data directly from the buffer in a third pass
1041         *  Make sure that we cannot end up on a non-event aligned position!
1042         */
1043         while(bytes2read>0) {
1044                 unsigned int framesread=(m_period*m_event_size-bytes2read)/m_event_size;
1045                 offset=framesread;
1046                
1047                 int bytesread=0;
1048
1049                 freebob_ringbuffer_get_read_vector(m_event_buffer, vec);
1050                        
1051                 if(vec[0].len==0) { // this indicates an empty event buffer
1052                         debugError("RCV: Event buffer underrun in processor %p\n",this);
1053                         break;
1054                 }
1055                        
1056                 /* if we don't take care we will get stuck in an infinite loop
1057                 * because we align to an event boundary later
1058                 * the remaining nb of bytes in one read operation can be smaller than one event
1059                 * this can happen because the ringbuffer size is always a power of 2
1060                         */
1061                 if(vec[0].len<m_event_size) {
1062                         // use the ringbuffer function to read one event
1063                         // the read function handles wrap around
1064                         freebob_ringbuffer_read(m_event_buffer,m_tmp_event_buffer,m_event_size);
1065
1066                         xrun = receiveBlock(m_tmp_event_buffer, 1, offset);
1067                                
1068                         if(xrun<0) {
1069                                 // xrun detected
1070                                 debugError("RCV: Frame buffer overrun in processor %p\n",this);
1071                                 break;
1072                         }
1073                                
1074                         // We advanced one m_event_size
1075                         bytes2read-=m_event_size;
1076                                
1077                 } else { //
1078                        
1079                         if(bytes2read>vec[0].len) {
1080                                         // align to an event boundary
1081                                 bytesread=vec[0].len-(vec[0].len%m_event_size);
1082                         } else {
1083                                 bytesread=bytes2read;
1084                         }
1085                                
1086                         xrun = receiveBlock(vec[0].buf, bytesread/m_event_size, offset);
1087                                
1088                         if(xrun<0) {
1089                                 // xrun detected
1090                                 debugError("RCV: Frame buffer overrun in processor %p\n",this);
1091                                 break;
1092                         }
1093
1094                         freebob_ringbuffer_read_advance(m_event_buffer, bytesread);
1095                         bytes2read -= bytesread;
1096                 }
1097                        
1098                 // the bytes2read should always be event aligned
1099                 assert(bytes2read%m_event_size==0);
1100         }
1101
1102         return true;
1103 }
1104
1105 /**
1106  * \brief write received events to the port ringbuffers.
1107  */
1108 int MotuReceiveStreamProcessor::receiveBlock(char *data,
1109                                            unsigned int nevents, unsigned int offset)
1110 {
1111         int problem=0;
1112
1113         /* Push cycle offset differences from each event's SPH into the DLL.
1114          * If this is the very first block received, use the first event to
1115          * initialise the last cycle offset.
1116          * FIXME: it might be best to use differences only within the given
1117          * block rather than keeping a store of the last cycle offset.
1118          * Otherwise in the event of a lost incoming packet the DLL will
1119          * have an abnormally large value sent to it.  Perhaps this doesn't
1120          * matter?
1121          */
1122         unsigned int ev;
1123         signed int sph_ofs = ntohl(*(quadlet_t *)data) & 0xfff;
1124
1125         if (m_last_cycle_ofs < 0) {
1126                 m_last_cycle_ofs = sph_ofs-(int)m_sph_ofs_dll->get();
1127         }
1128         for (ev=0; ev<nevents; ev++) {
1129                 sph_ofs = ntohl(*(quadlet_t *)(data+ev*m_event_size)) & 0xfff;
1130                 m_sph_ofs_dll->put((m_last_cycle_ofs<sph_ofs)?
1131                         sph_ofs-m_last_cycle_ofs:sph_ofs+3072-m_last_cycle_ofs);
1132                 m_last_cycle_ofs = sph_ofs;
1133         }
1134
1135         for ( PortVectorIterator it = m_PeriodPorts.begin();
1136           it != m_PeriodPorts.end();
1137           ++it ) {
1138                 if((*it)->isDisabled()) {continue;};
1139
1140                 //FIXME: make this into a static_cast when not DEBUG?
1141                 Port *port=dynamic_cast<Port *>(*it);
1142                
1143                 switch(port->getPortType()) {
1144                
1145                 case Port::E_Audio:
1146                         if(decodeMBLAEventsToPort(static_cast<MotuAudioPort *>(*it), (quadlet_t *)data, offset, nevents)) {
1147                                 debugWarning("Could not decode packet MBLA to port %s",(*it)->getName().c_str());
1148                                 problem=1;
1149                         }
1150                         break;
1151                 // midi is a packet based port, don't process
1152                 //      case MotuPortInfo::E_Midi:
1153                 //              break;
1154
1155                 default: // ignore
1156                         break;
1157                 }
1158         }
1159         return problem;
1160 }
1161
1162 /**
1163  * @brief decode a packet for the packet-based ports
1164  *
1165  * @param data Packet data
1166  * @param nevents number of events in data (including events of other ports & port types)
1167  * @param dbc DataBlockCount value for this packet
1168  * @return true if all successfull
1169  */
1170 bool MotuReceiveStreamProcessor::decodePacketPorts(quadlet_t *data, unsigned int nevents, unsigned int dbc)
1171 {
1172         bool ok=true;
1173        
1174         quadlet_t *target_event=NULL;
1175         int j;
1176        
1177         for ( PortVectorIterator it = m_PacketPorts.begin();
1178           it != m_PacketPorts.end();
1179           ++it ) {
1180
1181 #ifdef DEBUG
1182                 MotuPortInfo *pinfo=dynamic_cast<MotuPortInfo *>(*it);
1183                 assert(pinfo); // this should not fail!!
1184
1185                 // the only packet type of events for AMDTP is MIDI in mbla
1186 //              assert(pinfo->getFormat()==MotuPortInfo::E_Midi);
1187 #endif
1188                 MotuMidiPort *mp=static_cast<MotuMidiPort *>(*it);
1189                
1190
1191         // do decoding here
1192
1193         }
1194        
1195         return ok;
1196 }
1197
1198 signed int MotuReceiveStreamProcessor::decodeMBLAEventsToPort(MotuAudioPort *p,
1199                 quadlet_t *data, unsigned int offset, unsigned int nevents)
1200 {
1201         unsigned int j=0;
1202
1203 //      printf("****************\n");
1204 //      hexDumpQuadlets(data,m_dimension*4);
1205 //      printf("****************\n");
1206
1207         // Use char here since a port's source address won't necessarily be
1208         // aligned; use of an unaligned quadlet_t may cause issues on certain
1209         // architectures.
1210         unsigned char *src_data;
1211         src_data = (unsigned char *)data + p->getPosition();
1212
1213         switch(p->getDataType()) {
1214                 default:
1215                 case Port::E_Int24:
1216                         {
1217                                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
1218
1219                                 assert(nevents + offset <= p->getBufferSize());
1220
1221                                 // Offset is in frames, but each port is only a single
1222                                 // channel, so the number of frames is the same as the
1223                                 // number of quadlets to offset (assuming the port buffer
1224                                 // uses one quadlet per sample, which is the case currently).
1225                                 buffer+=offset;
1226
1227                                 for(j = 0; j < nevents; j += 1) { // Decode nsamples
1228                                         *buffer = (*src_data<<16)+(*(src_data+1)<<8)+*(src_data+2);
1229                                         // Sign-extend highest bit of 24-bit int.
1230                                         // FIXME: this isn't strictly needed since E_Int24 is a 24-bit,
1231                                         // but doing so shouldn't break anything and makes the data
1232                                         // easier to deal with during debugging.
1233                                         if (*src_data & 0x80)
1234                                                 *buffer |= 0xff000000;
1235
1236                                         buffer++;
1237                                         src_data+=m_event_size;
1238                                 }
1239                         }
1240                         break;
1241                 case Port::E_Float:
1242                         {
1243                                 const float multiplier = 1.0f / (float)(0x7FFFFF);
1244                                 float *buffer=(float *)(p->getBufferAddress());
1245
1246                                 assert(nevents + offset <= p->getBufferSize());
1247
1248                                 buffer+=offset;
1249
1250                                 for(j = 0; j < nevents; j += 1) { // decode max nsamples               
1251        
1252                                         unsigned int v = (*src_data<<16)+(*(src_data+1)<<8)+*(src_data+2);
1253
1254                                         // sign-extend highest bit of 24-bit int
1255                                         int tmp = (int)(v << 8) / 256;
1256                
1257                                         *buffer = tmp * multiplier;
1258                                
1259                                         buffer++;
1260                                         src_data+=m_event_size;
1261                                 }
1262                         }
1263                         break;
1264         }
1265
1266         return 0;
1267 }
1268
1269 signed int MotuReceiveStreamProcessor::setEventSize(unsigned int size) {
1270         m_event_size = size;
1271         return 0;
1272 }
1273
1274 unsigned int MotuReceiveStreamProcessor::getEventSize(void) {
1275 //
1276 // Return the size of a single event sent by the MOTU as part of an iso
1277 // data packet in bytes.
1278 //
1279         return m_event_size;
1280 }
1281                
1282 } // end of namespace FreebobStreaming
Note: See TracBrowser for help on using the browser.