root/branches/libffado-2.0/src/libstreaming/amdtp/AmdtpReceiveStreamProcessor.cpp

Revision 1530, 16.5 kB (checked in by ppalmers, 12 years ago)

- make the sync delay an integer amount of packets to avoid phase issues
- debug message updates

Line 
1 /*
2  * Copyright (C) 2005-2008 by Pieter Palmers
3  *
4  * This file is part of FFADO
5  * FFADO = Free Firewire (pro-)audio drivers for linux
6  *
7  * FFADO is based upon FreeBoB.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 2 of the License, or
12  * (at your option) version 3 of the License.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
21  *
22  */
23 #include "config.h"
24
25 #include "AmdtpReceiveStreamProcessor.h"
26 #include "AmdtpPort.h"
27 #include "../StreamProcessorManager.h"
28 #include "devicemanager.h"
29
30 #include "libieee1394/ieee1394service.h"
31 #include "libieee1394/IsoHandlerManager.h"
32 #include "libieee1394/cycletimer.h"
33
34 #include "libutil/ByteSwap.h"
35 #include <assert.h>
36 #include "libutil/SystemTimeSource.h"
37
38 namespace Streaming {
39
40 /* --------------------- RECEIVE ----------------------- */
41
42 AmdtpReceiveStreamProcessor::AmdtpReceiveStreamProcessor(FFADODevice &parent, int dimension)
43     : StreamProcessor(parent, ePT_Receive)
44     , m_dimension( dimension )
45     , m_nb_audio_ports( 0 )
46     , m_nb_midi_ports( 0 )
47 {}
48
49 unsigned int
50 AmdtpReceiveStreamProcessor::getSytInterval() {
51     switch (m_StreamProcessorManager.getNominalRate()) {
52         case 32000:
53         case 44100:
54         case 48000:
55             return 8;
56         case 88200:
57         case 96000:
58             return 16;
59         case 176400:
60         case 192000:
61             return 32;
62         default:
63             debugError("Unsupported rate: %d\n", m_StreamProcessorManager.getNominalRate());
64             return 0;
65     }
66 }
67
68 bool AmdtpReceiveStreamProcessor::prepareChild() {
69     debugOutput( DEBUG_LEVEL_VERBOSE, "Preparing (%p)...\n", this);
70     m_syt_interval = getSytInterval();
71
72     if (!initPortCache()) {
73         debugError("Could not init port cache\n");
74         return false;
75     }
76
77     return true;
78 }
79
80
81 /**
82  * Processes packet header to extract timestamps and so on
83  * @param data
84  * @param length
85  * @param channel
86  * @param tag
87  * @param sy
88  * @param pkt_ctr CTR value when packet was received
89  * @return
90  */
91 enum StreamProcessor::eChildReturnValue
92 AmdtpReceiveStreamProcessor::processPacketHeader(unsigned char *data, unsigned int length,
93                                                  unsigned char tag, unsigned char sy,
94                                                  uint32_t pkt_ctr)
95 {
96     struct iec61883_packet *packet = (struct iec61883_packet *) data;
97     assert(packet);
98     bool ok = (packet->syt != 0xFFFF) &&
99               (packet->fdf != 0xFF) &&
100               (packet->fmt == 0x10) &&
101               (packet->dbs > 0) &&
102               (length >= 2*sizeof(quadlet_t));
103     if(ok) {
104         m_last_timestamp = sytRecvToFullTicks2((uint32_t)CondSwapFromBus16(packet->syt), pkt_ctr);
105         //#ifdef DEBUG
106         #if 0
107         uint32_t now = m_1394service.getCycleTimer();
108
109         //=> convert the SYT to a full timestamp in ticks
110         uint64_t old_last_timestamp = sytRecvToFullTicks((uint32_t)CondSwapFromBus16(packet->syt),
111                                               CYCLE_TIMER_GET_CYCLES(pkt_ctr), now);
112         if(m_last_timestamp != old_last_timestamp) {
113             debugWarning("discepancy between timestamp calculations\n");
114         }
115         #endif
116     }
117     return (ok ? eCRV_OK : eCRV_Invalid );
118 }
119
120 /**
121  * extract the data from the packet
122  * @pre the IEC61883 packet is valid according to isValidPacket
123  * @param data
124  * @param length
125  * @param channel
126  * @param tag
127  * @param sy
128  * @param pkt_ctr
129  * @return
130  */
131 enum StreamProcessor::eChildReturnValue
132 AmdtpReceiveStreamProcessor::processPacketData(unsigned char *data, unsigned int length) {
133     struct iec61883_packet *packet = (struct iec61883_packet *) data;
134     assert(packet);
135
136     unsigned int nevents=((length / sizeof (quadlet_t)) - 2)/packet->dbs;
137
138     // we have to keep in mind that there are also
139     // some packets buffered by the ISO layer,
140     // at most x=m_handler->getWakeupInterval()
141     // these contain at most x*syt_interval
142     // frames, meaning that we might receive
143     // this packet x*syt_interval*ticks_per_frame
144     // later than expected (the real receive time)
145     #ifdef DEBUG
146     static int64_t last_t = Util::SystemTimeSource::getCurrentTime();
147     int64_t now_t = Util::SystemTimeSource::getCurrentTime();
148     if(isRunning()) {
149         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
150                            "STMP: %lluticks | syt_interval=%d, tpf=%f\n",
151                            m_last_timestamp, m_syt_interval, getTicksPerFrame());
152 /*        debugOutput(DEBUG_LEVEL_NORMAL,
153                            "STMP: %12llu ticks | delta_t: %5lld | bufferfill: %5d\n",
154                            m_last_timestamp, now_t-last_t, m_data_buffer->getBufferFill());*/
155     }
156     last_t = now_t;
157
158     // check whether nevents is a multiple of 8.
159     if (nevents & 0x7) {
160         debugError("Invalid nevents value for AMDTP (%u)\n", nevents);
161     }
162     #endif
163
164     if(m_data_buffer->writeFrames(nevents, (char *)(data+8), m_last_timestamp)) {
165         return eCRV_OK;
166     } else {
167         return eCRV_XRun;
168     }
169 }
170
171 /***********************************************
172  * Encoding/Decoding API                       *
173  ***********************************************/
174 /**
175  * @brief write received events to the stream ringbuffers.
176  */
177 bool AmdtpReceiveStreamProcessor::processReadBlock(char *data,
178                        unsigned int nevents, unsigned int offset)
179 {
180     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE,
181                         "(%p)->processReadBlock(%u, %u)\n",
182                         this, nevents, offset);
183
184     // update the variable parts of the cache
185     updatePortCache();
186
187     // decode audio data
188     switch(m_StreamProcessorManager.getAudioDataType()) {
189         case StreamProcessorManager::eADT_Int24:
190             decodeAudioPortsInt24((quadlet_t *)data, offset, nevents);
191             break;
192         case StreamProcessorManager::eADT_Float:
193             decodeAudioPortsFloat((quadlet_t *)data, offset, nevents);
194             break;
195     }
196
197     // do midi ports
198     decodeMidiPorts((quadlet_t *)data, offset, nevents);
199     return true;
200 }
201
202 //#ifdef __SSE2__
203 #if 0 // SSE code is not ready yet
204 #include <emmintrin.h>
205 #warning SSE2 build
206
207 /**
208  * @brief demux events to all audio ports (int24)
209  * @param data
210  * @param offset
211  * @param nevents
212  */
213 void
214 AmdtpReceiveStreamProcessor::decodeAudioPortsInt24(quadlet_t *data,
215                                                     unsigned int offset,
216                                                     unsigned int nevents)
217 {
218     unsigned int j;
219     quadlet_t *target_event;
220     unsigned int i;
221
222     for (i = 0; i < m_nb_audio_ports; i++) {
223         struct _MBLA_port_cache &p = m_audio_ports.at(i);
224         target_event = (quadlet_t *)(data + i);
225         assert(nevents + offset <= p.buffer_size );
226
227         if(p.buffer && p.enabled) {
228             quadlet_t *buffer = (quadlet_t *)(p.buffer);
229             buffer += offset;
230
231             for(j = 0; j < nevents; j += 1) {
232                 *(buffer)=(CondSwapFromBus32((*target_event) ) & 0x00FFFFFF);
233                 buffer++;
234                 target_event+=m_dimension;
235             }
236         }
237     }
238 }
239
240 /**
241  * @brief demux events to all audio ports (float)
242  * @param data
243  * @param offset
244  * @param nevents
245  */
246 void
247 AmdtpReceiveStreamProcessor::decodeAudioPortsFloat(quadlet_t *data,
248                                                     unsigned int offset,
249                                                     unsigned int nevents)
250 {
251     unsigned int j;
252     quadlet_t *target_event;
253     unsigned int i;
254     const float multiplier = 1.0f / (float)(0x7FFFFF);
255
256     for (i = 0; i < m_nb_audio_ports; i++) {
257         struct _MBLA_port_cache &p = m_audio_ports.at(i);
258         target_event = (quadlet_t *)(data + i);
259         assert(nevents + offset <= p.buffer_size );
260
261         if(p.buffer && p.enabled) {
262             float *buffer = (float *)(p.buffer);
263             buffer += offset;
264
265             for(j = 0; j < nevents; j += 1) {
266                 unsigned int v = CondSwapFromBus32(*target_event) & 0x00FFFFFF;
267                 // sign-extend highest bit of 24-bit int
268                 int tmp = (int)(v << 8) / 256;
269                 *buffer = tmp * multiplier;
270                 buffer++;
271                 target_event+=m_dimension;
272             }
273         }
274     }
275 }
276
277 #else
278 /**
279  * @brief demux events to all audio ports (int24)
280  * @param data
281  * @param offset
282  * @param nevents
283  */
284 void
285 AmdtpReceiveStreamProcessor::decodeAudioPortsInt24(quadlet_t *data,
286                                                     unsigned int offset,
287                                                     unsigned int nevents)
288 {
289     unsigned int j;
290     quadlet_t *target_event;
291     unsigned int i;
292
293     for (i = 0; i < m_nb_audio_ports; i++) {
294         struct _MBLA_port_cache &p = m_audio_ports.at(i);
295         target_event = (quadlet_t *)(data + i);
296         assert(nevents + offset <= p.buffer_size );
297
298         if(p.buffer && p.enabled) {
299             quadlet_t *buffer = (quadlet_t *)(p.buffer);
300             buffer += offset;
301
302             for(j = 0; j < nevents; j += 1) {
303                 *(buffer)=(CondSwapFromBus32((*target_event) ) & 0x00FFFFFF);
304                 buffer++;
305                 target_event+=m_dimension;
306             }
307         }
308     }
309 }
310
311 /**
312  * @brief demux events to all audio ports (float)
313  * @param data
314  * @param offset
315  * @param nevents
316  */
317 void
318 AmdtpReceiveStreamProcessor::decodeAudioPortsFloat(quadlet_t *data,
319                                                     unsigned int offset,
320                                                     unsigned int nevents)
321 {
322     unsigned int j;
323     quadlet_t *target_event;
324     unsigned int i;
325     const float multiplier = 1.0f / (float)(0x7FFFFF);
326
327     for (i = 0; i < m_nb_audio_ports; i++) {
328         struct _MBLA_port_cache &p = m_audio_ports.at(i);
329         target_event = (quadlet_t *)(data + i);
330         assert(nevents + offset <= p.buffer_size );
331
332         if(p.buffer && p.enabled) {
333             float *buffer = (float *)(p.buffer);
334             buffer += offset;
335
336             for(j = 0; j < nevents; j += 1) {
337                 unsigned int v = CondSwapFromBus32(*target_event) & 0x00FFFFFF;
338                 // sign-extend highest bit of 24-bit int
339                 int tmp = (int)(v << 8) / 256;
340                 *buffer = tmp * multiplier;
341                 buffer++;
342                 target_event+=m_dimension;
343             }
344         }
345     }
346 }
347
348 #endif
349
350 /**
351  * @brief decode all midi ports in the cache from events
352  * @param data
353  * @param offset
354  * @param nevents
355  */
356 void
357 AmdtpReceiveStreamProcessor::decodeMidiPorts(quadlet_t *data,
358                                               unsigned int offset,
359                                               unsigned int nevents)
360 {
361     quadlet_t *target_event;
362     quadlet_t sample_int;
363     unsigned int i,j;
364
365     for (i = 0; i < m_nb_midi_ports; i++) {
366         struct _MIDI_port_cache &p = m_midi_ports.at(i);
367         if (p.buffer && p.enabled) {
368             uint32_t *buffer = (quadlet_t *)(p.buffer);
369             buffer += offset;
370             for (j = p.location;j < nevents; j += 8) {
371                 target_event = (quadlet_t *) (data + ((j * m_dimension) + p.position));
372                 sample_int = CondSwapFromBus32(*target_event);
373
374                 // FIXME: this assumes that 2X and 3X speed isn't used,
375                 // because only the 1X slot is put into the ringbuffer
376                 if(IEC61883_AM824_HAS_LABEL(sample_int, IEC61883_AM824_LABEL_MIDI_1X)) {
377                     sample_int=(sample_int >> 16) & 0x000000FF;
378                     sample_int |= 0x01000000; // flag that there is a midi event present
379                     *buffer = sample_int;
380                 } else if(IEC61883_AM824_HAS_LABEL(sample_int, IEC61883_AM824_LABEL_MIDI_2X)
381                        || IEC61883_AM824_HAS_LABEL(sample_int, IEC61883_AM824_LABEL_MIDI_3X) ) {
382                     debugOutput(DEBUG_LEVEL_VERBOSE, "Midi mode %X not supported.\n", IEC61883_AM824_GET_LABEL(sample_int));
383                 } else {
384                     // make sure no event is received
385                     *buffer = 0;
386                 }
387                 buffer+=8;
388             }
389         }
390     }
391 }
392
393 bool
394 AmdtpReceiveStreamProcessor::initPortCache() {
395     // make use of the fact that audio ports are the first ports in
396     // the cluster as per AMDTP. so we can sort the ports by position
397     // and have very efficient lookups:
398     // m_float_ports.at(i).buffer -> audio stream i buffer
399     // for midi ports we simply cache all port info since they are (usually) not
400     // that numerous
401     m_nb_audio_ports = 0;
402     m_audio_ports.clear();
403    
404     m_nb_midi_ports = 0;
405     m_midi_ports.clear();
406    
407     for(PortVectorIterator it = m_Ports.begin();
408         it != m_Ports.end();
409         ++it )
410     {
411         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
412         assert(pinfo); // this should not fail!!
413
414         switch( pinfo->getFormat() )
415         {
416             case AmdtpPortInfo::E_MBLA:
417                 m_nb_audio_ports++;
418                 break;
419             case AmdtpPortInfo::E_SPDIF: // still unimplemented
420                 break;
421             case AmdtpPortInfo::E_Midi:
422                 m_nb_midi_ports++;
423                 break;
424             default: // ignore
425                 break;
426         }
427     }
428
429     unsigned int idx;
430     for (idx = 0; idx < m_nb_audio_ports; idx++) {
431         for(PortVectorIterator it = m_Ports.begin();
432             it != m_Ports.end();
433             ++it )
434         {
435             AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
436             debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
437                         "idx %u: looking at port %s at position %u\n",
438                         idx, (*it)->getName().c_str(), pinfo->getPosition());
439             if(pinfo->getPosition() == idx) {
440                 struct _MBLA_port_cache p;
441                 p.port = dynamic_cast<AmdtpAudioPort *>(*it);
442                 if(p.port == NULL) {
443                     debugError("Port is not an AmdtpAudioPort!\n");
444                     return false;
445                 }
446                 p.buffer = NULL; // to be filled by updatePortCache
447                 #ifdef DEBUG
448                 p.buffer_size = (*it)->getBufferSize();
449                 #endif
450
451                 m_audio_ports.push_back(p);
452                 debugOutput(DEBUG_LEVEL_VERBOSE,
453                             "Cached port %s at position %u\n",
454                             p.port->getName().c_str(), idx);
455                 goto next_index;
456             }
457         }
458         debugError("No MBLA port found for position %d\n", idx);
459         return false;
460 next_index:
461         continue;
462     }
463
464     for(PortVectorIterator it = m_Ports.begin();
465         it != m_Ports.end();
466         ++it )
467     {
468         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
469         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
470                     "idx %u: looking at port %s at position %u, location %u\n",
471                     idx, (*it)->getName().c_str(), pinfo->getPosition(), pinfo->getLocation());
472         if ((*it)->getPortType() == Port::E_Midi) {
473             struct _MIDI_port_cache p;
474             p.port = dynamic_cast<AmdtpMidiPort *>(*it);
475             if(p.port == NULL) {
476                 debugError("Port is not an AmdtpMidiPort!\n");
477                 return false;
478             }
479             p.position = pinfo->getPosition();
480             p.location = pinfo->getLocation();
481             p.buffer = NULL; // to be filled by updatePortCache
482             #ifdef DEBUG
483             p.buffer_size = (*it)->getBufferSize();
484             #endif
485
486             m_midi_ports.push_back(p);
487             debugOutput(DEBUG_LEVEL_VERBOSE,
488                         "Cached port %s at position %u, location %u\n",
489                         p.port->getName().c_str(), p.position, p.location);
490         }
491     }
492
493     return true;
494 }
495
496 void
497 AmdtpReceiveStreamProcessor::updatePortCache() {
498     unsigned int idx;
499     for (idx = 0; idx < m_nb_audio_ports; idx++) {
500         struct _MBLA_port_cache& p = m_audio_ports.at(idx);
501         AmdtpAudioPort *port = p.port;
502         p.buffer = port->getBufferAddress();
503         p.enabled = !port->isDisabled();
504     }
505     for (idx = 0; idx < m_nb_midi_ports; idx++) {
506         struct _MIDI_port_cache& p = m_midi_ports.at(idx);
507         AmdtpMidiPort *port = p.port;
508         p.buffer = port->getBufferAddress();
509         p.enabled = !port->isDisabled();
510     }
511 }
512
513 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.