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

Revision 283, 37.4 kB (checked in by jwoithe, 18 years ago)

More additions for MOTU playback (transmit) functionality.
Bugfix for MOTU stream stop method.

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