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

Revision 1544, 16.6 kB (checked in by ppalmers, 12 years ago)

round the transmit safety buffer size to one packet size (in frames) to avoid messing up the MIDI time muxed stream position

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                     debugOutputExtreme(DEBUG_LEVEL_VERBOSE, "(%p) MIDI [%d]: %08X\n", this, i, sample_int);
381                 } else if(IEC61883_AM824_HAS_LABEL(sample_int, IEC61883_AM824_LABEL_MIDI_2X)
382                        || IEC61883_AM824_HAS_LABEL(sample_int, IEC61883_AM824_LABEL_MIDI_3X) ) {
383                     debugOutput(DEBUG_LEVEL_VERBOSE, "Midi mode %X not supported.\n", IEC61883_AM824_GET_LABEL(sample_int));
384                 } else {
385                     // make sure no event is received
386                     *buffer = 0;
387                 }
388                 buffer+=8;
389             }
390         }
391     }
392 }
393
394 bool
395 AmdtpReceiveStreamProcessor::initPortCache() {
396     // make use of the fact that audio ports are the first ports in
397     // the cluster as per AMDTP. so we can sort the ports by position
398     // and have very efficient lookups:
399     // m_float_ports.at(i).buffer -> audio stream i buffer
400     // for midi ports we simply cache all port info since they are (usually) not
401     // that numerous
402     m_nb_audio_ports = 0;
403     m_audio_ports.clear();
404    
405     m_nb_midi_ports = 0;
406     m_midi_ports.clear();
407    
408     for(PortVectorIterator it = m_Ports.begin();
409         it != m_Ports.end();
410         ++it )
411     {
412         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
413         assert(pinfo); // this should not fail!!
414
415         switch( pinfo->getFormat() )
416         {
417             case AmdtpPortInfo::E_MBLA:
418                 m_nb_audio_ports++;
419                 break;
420             case AmdtpPortInfo::E_SPDIF: // still unimplemented
421                 break;
422             case AmdtpPortInfo::E_Midi:
423                 m_nb_midi_ports++;
424                 break;
425             default: // ignore
426                 break;
427         }
428     }
429
430     unsigned int idx;
431     for (idx = 0; idx < m_nb_audio_ports; idx++) {
432         for(PortVectorIterator it = m_Ports.begin();
433             it != m_Ports.end();
434             ++it )
435         {
436             AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
437             debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
438                         "idx %u: looking at port %s at position %u\n",
439                         idx, (*it)->getName().c_str(), pinfo->getPosition());
440             if(pinfo->getPosition() == idx) {
441                 struct _MBLA_port_cache p;
442                 p.port = dynamic_cast<AmdtpAudioPort *>(*it);
443                 if(p.port == NULL) {
444                     debugError("Port is not an AmdtpAudioPort!\n");
445                     return false;
446                 }
447                 p.buffer = NULL; // to be filled by updatePortCache
448                 #ifdef DEBUG
449                 p.buffer_size = (*it)->getBufferSize();
450                 #endif
451
452                 m_audio_ports.push_back(p);
453                 debugOutput(DEBUG_LEVEL_VERBOSE,
454                             "Cached port %s at position %u\n",
455                             p.port->getName().c_str(), idx);
456                 goto next_index;
457             }
458         }
459         debugError("No MBLA port found for position %d\n", idx);
460         return false;
461 next_index:
462         continue;
463     }
464
465     for(PortVectorIterator it = m_Ports.begin();
466         it != m_Ports.end();
467         ++it )
468     {
469         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
470         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
471                     "idx %u: looking at port %s at position %u, location %u\n",
472                     idx, (*it)->getName().c_str(), pinfo->getPosition(), pinfo->getLocation());
473         if ((*it)->getPortType() == Port::E_Midi) {
474             struct _MIDI_port_cache p;
475             p.port = dynamic_cast<AmdtpMidiPort *>(*it);
476             if(p.port == NULL) {
477                 debugError("Port is not an AmdtpMidiPort!\n");
478                 return false;
479             }
480             p.position = pinfo->getPosition();
481             p.location = pinfo->getLocation();
482             p.buffer = NULL; // to be filled by updatePortCache
483             #ifdef DEBUG
484             p.buffer_size = (*it)->getBufferSize();
485             #endif
486
487             m_midi_ports.push_back(p);
488             debugOutput(DEBUG_LEVEL_VERBOSE,
489                         "Cached port %s at position %u, location %u\n",
490                         p.port->getName().c_str(), p.position, p.location);
491         }
492     }
493
494     return true;
495 }
496
497 void
498 AmdtpReceiveStreamProcessor::updatePortCache() {
499     unsigned int idx;
500     for (idx = 0; idx < m_nb_audio_ports; idx++) {
501         struct _MBLA_port_cache& p = m_audio_ports.at(idx);
502         AmdtpAudioPort *port = p.port;
503         p.buffer = port->getBufferAddress();
504         p.enabled = !port->isDisabled();
505     }
506     for (idx = 0; idx < m_nb_midi_ports; idx++) {
507         struct _MIDI_port_cache& p = m_midi_ports.at(idx);
508         AmdtpMidiPort *port = p.port;
509         p.buffer = port->getBufferAddress();
510         p.enabled = !port->isDisabled();
511     }
512 }
513
514 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.