root/trunk/libffado/src/libstreaming/amdtp/AmdtpReceiveStreamProcessor.cpp

Revision 1348, 16.7 kB (checked in by ppalmers, 16 years ago)

merge 2.0 branch changes to trunk. svn merge -r 1337:HEAD svn+ssh://ffadosvn@ffado.org/ffado/branches/libffado-2.0

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