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

Revision 1254, 15.9 kB (checked in by ppalmers, 16 years ago)

split config.h into config/version/debug_config to allow for faster compilation (splits dependencies)

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
37 namespace Streaming {
38
39 /* --------------------- RECEIVE ----------------------- */
40
41 AmdtpReceiveStreamProcessor::AmdtpReceiveStreamProcessor(FFADODevice &parent, int dimension)
42     : StreamProcessor(parent, ePT_Receive)
43     , m_dimension( dimension )
44     , m_nb_audio_ports( 0 )
45     , m_nb_midi_ports( 0 )
46
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     if(isRunning()) {
147         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
148                            "STMP: %lluticks | syt_interval=%d, tpf=%f\n",
149                            m_last_timestamp, m_syt_interval, getTicksPerFrame());
150     }
151
152     // check whether nevents is a multiple of 8.
153     if (nevents & 0x7) {
154         debugError("Invalid nevents value for AMDTP (%u)\n", nevents);
155     }
156     #endif
157
158     if(m_data_buffer->writeFrames(nevents, (char *)(data+8), m_last_timestamp)) {
159         return eCRV_OK;
160     } else {
161         return eCRV_XRun;
162     }
163 }
164
165 /***********************************************
166  * Encoding/Decoding API                       *
167  ***********************************************/
168 /**
169  * @brief write received events to the stream ringbuffers.
170  */
171 bool AmdtpReceiveStreamProcessor::processReadBlock(char *data,
172                        unsigned int nevents, unsigned int offset)
173 {
174     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE,
175                         "(%p)->processReadBlock(%u, %u)\n",
176                         this,nevents,offset);
177
178     // update the variable parts of the cache
179     updatePortCache();
180
181     // decode audio data
182     switch(m_StreamProcessorManager.getAudioDataType()) {
183         case StreamProcessorManager::eADT_Int24:
184             decodeAudioPortsInt24((quadlet_t *)data, offset, nevents);
185             break;
186         case StreamProcessorManager::eADT_Float:
187             decodeAudioPortsFloat((quadlet_t *)data, offset, nevents);
188             break;
189     }
190
191     // do midi ports
192     decodeMidiPorts((quadlet_t *)data, offset, nevents);
193     return true;
194 }
195
196 //#ifdef __SSE2__
197 #if 0 // SSE code is not ready yet
198 #include <emmintrin.h>
199 #warning SSE2 build
200
201 /**
202  * @brief demux events to all audio ports (int24)
203  * @param data
204  * @param offset
205  * @param nevents
206  */
207 void
208 AmdtpReceiveStreamProcessor::decodeAudioPortsInt24(quadlet_t *data,
209                                                     unsigned int offset,
210                                                     unsigned int nevents)
211 {
212     unsigned int j;
213     quadlet_t *target_event;
214     unsigned int i;
215
216     for (i = 0; i < m_nb_audio_ports; i++) {
217         struct _MBLA_port_cache &p = m_audio_ports.at(i);
218         target_event = (quadlet_t *)(data + i);
219         assert(nevents + offset <= p.buffer_size );
220
221         if(p.buffer && p.enabled) {
222             quadlet_t *buffer = (quadlet_t *)(p.buffer);
223             buffer += offset;
224
225             for(j = 0; j < nevents; j += 1) {
226                 *(buffer)=(CondSwapFromBus32((*target_event) ) & 0x00FFFFFF);
227                 buffer++;
228                 target_event+=m_dimension;
229             }
230         }
231     }
232 }
233
234 /**
235  * @brief demux events to all audio ports (float)
236  * @param data
237  * @param offset
238  * @param nevents
239  */
240 void
241 AmdtpReceiveStreamProcessor::decodeAudioPortsFloat(quadlet_t *data,
242                                                     unsigned int offset,
243                                                     unsigned int nevents)
244 {
245     unsigned int j;
246     quadlet_t *target_event;
247     unsigned int i;
248     const float multiplier = 1.0f / (float)(0x7FFFFF);
249
250     for (i = 0; i < m_nb_audio_ports; i++) {
251         struct _MBLA_port_cache &p = m_audio_ports.at(i);
252         target_event = (quadlet_t *)(data + i);
253         assert(nevents + offset <= p.buffer_size );
254
255         if(p.buffer && p.enabled) {
256             float *buffer = (float *)(p.buffer);
257             buffer += offset;
258
259             for(j = 0; j < nevents; j += 1) {
260                 unsigned int v = CondSwapFromBus32(*target_event) & 0x00FFFFFF;
261                 // sign-extend highest bit of 24-bit int
262                 int tmp = (int)(v << 8) / 256;
263                 *buffer = tmp * multiplier;
264                 buffer++;
265                 target_event+=m_dimension;
266             }
267         }
268     }
269 }
270
271 #else
272 /**
273  * @brief demux events to all audio ports (int24)
274  * @param data
275  * @param offset
276  * @param nevents
277  */
278 void
279 AmdtpReceiveStreamProcessor::decodeAudioPortsInt24(quadlet_t *data,
280                                                     unsigned int offset,
281                                                     unsigned int nevents)
282 {
283     unsigned int j;
284     quadlet_t *target_event;
285     unsigned int i;
286
287     for (i = 0; i < m_nb_audio_ports; i++) {
288         struct _MBLA_port_cache &p = m_audio_ports.at(i);
289         target_event = (quadlet_t *)(data + i);
290         assert(nevents + offset <= p.buffer_size );
291
292         if(p.buffer && p.enabled) {
293             quadlet_t *buffer = (quadlet_t *)(p.buffer);
294             buffer += offset;
295
296             for(j = 0; j < nevents; j += 1) {
297                 *(buffer)=(CondSwapFromBus32((*target_event) ) & 0x00FFFFFF);
298                 buffer++;
299                 target_event+=m_dimension;
300             }
301         }
302     }
303 }
304
305 /**
306  * @brief demux events to all audio ports (float)
307  * @param data
308  * @param offset
309  * @param nevents
310  */
311 void
312 AmdtpReceiveStreamProcessor::decodeAudioPortsFloat(quadlet_t *data,
313                                                     unsigned int offset,
314                                                     unsigned int nevents)
315 {
316     unsigned int j;
317     quadlet_t *target_event;
318     unsigned int i;
319     const float multiplier = 1.0f / (float)(0x7FFFFF);
320
321     for (i = 0; i < m_nb_audio_ports; i++) {
322         struct _MBLA_port_cache &p = m_audio_ports.at(i);
323         target_event = (quadlet_t *)(data + i);
324         assert(nevents + offset <= p.buffer_size );
325
326         if(p.buffer && p.enabled) {
327             float *buffer = (float *)(p.buffer);
328             buffer += offset;
329
330             for(j = 0; j < nevents; j += 1) {
331                 unsigned int v = CondSwapFromBus32(*target_event) & 0x00FFFFFF;
332                 // sign-extend highest bit of 24-bit int
333                 int tmp = (int)(v << 8) / 256;
334                 *buffer = tmp * multiplier;
335                 buffer++;
336                 target_event+=m_dimension;
337             }
338         }
339     }
340 }
341
342 #endif
343
344 /**
345  * @brief decode all midi ports in the cache from events
346  * @param data
347  * @param offset
348  * @param nevents
349  */
350 void
351 AmdtpReceiveStreamProcessor::decodeMidiPorts(quadlet_t *data,
352                                               unsigned int offset,
353                                               unsigned int nevents)
354 {
355     quadlet_t *target_event;
356     quadlet_t sample_int;
357     unsigned int i,j;
358
359     for (i = 0; i < m_nb_midi_ports; i++) {
360         struct _MIDI_port_cache &p = m_midi_ports.at(i);
361         if (p.buffer && p.enabled) {
362             uint32_t *buffer = (quadlet_t *)(p.buffer);
363             buffer += offset;
364
365             for (j = p.location;j < nevents; j += 8) {
366                 target_event = (quadlet_t *) (data + ((j * m_dimension) + p.position));
367                 sample_int=CondSwapFromBus32(*target_event);
368                 // FIXME: this assumes that 2X and 3X speed isn't used,
369                 // because only the 1X slot is put into the ringbuffer
370                 if(IEC61883_AM824_GET_LABEL(sample_int) != IEC61883_AM824_LABEL_MIDI_NO_DATA) {
371                     sample_int=(sample_int >> 16) & 0x000000FF;
372                     sample_int |= 0x01000000; // flag that there is a midi event present
373                     *buffer = sample_int;
374                     debugOutput(DEBUG_LEVEL_VERBOSE, "Received midi byte %08X on port %p index %d\n", sample_int, p, j-p.location);
375                 } else {
376                     // make sure no event is received
377                     *buffer = 0;
378                 }
379                 buffer+=8;
380             }
381         }
382     }
383 }
384
385 bool
386 AmdtpReceiveStreamProcessor::initPortCache() {
387     // make use of the fact that audio ports are the first ports in
388     // the cluster as per AMDTP. so we can sort the ports by position
389     // and have very efficient lookups:
390     // m_float_ports.at(i).buffer -> audio stream i buffer
391     // for midi ports we simply cache all port info since they are (usually) not
392     // that numerous
393     m_nb_audio_ports = 0;
394     m_audio_ports.clear();
395    
396     m_nb_midi_ports = 0;
397     m_midi_ports.clear();
398    
399     for(PortVectorIterator it = m_Ports.begin();
400         it != m_Ports.end();
401         ++it )
402     {
403         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
404         assert(pinfo); // this should not fail!!
405
406         switch( pinfo->getFormat() )
407         {
408             case AmdtpPortInfo::E_MBLA:
409                 m_nb_audio_ports++;
410                 break;
411             case AmdtpPortInfo::E_SPDIF: // still unimplemented
412                 break;
413             case AmdtpPortInfo::E_Midi:
414                 m_nb_midi_ports++;
415                 break;
416             default: // ignore
417                 break;
418         }
419     }
420
421     unsigned int idx;
422     for (idx = 0; idx < m_nb_audio_ports; idx++) {
423         for(PortVectorIterator it = m_Ports.begin();
424             it != m_Ports.end();
425             ++it )
426         {
427             AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
428             debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
429                         "idx %u: looking at port %s at position %u\n",
430                         idx, (*it)->getName().c_str(), pinfo->getPosition());
431             if(pinfo->getPosition() == idx) {
432                 struct _MBLA_port_cache p;
433                 p.port = dynamic_cast<AmdtpAudioPort *>(*it);
434                 if(p.port == NULL) {
435                     debugError("Port is not an AmdtpAudioPort!\n");
436                     return false;
437                 }
438                 p.buffer = NULL; // to be filled by updatePortCache
439                 #ifdef DEBUG
440                 p.buffer_size = (*it)->getBufferSize();
441                 #endif
442
443                 m_audio_ports.push_back(p);
444                 debugOutput(DEBUG_LEVEL_VERBOSE,
445                             "Cached port %s at position %u\n",
446                             p.port->getName().c_str(), idx);
447                 goto next_index;
448             }
449         }
450         debugError("No MBLA port found for position %d\n", idx);
451         return false;
452 next_index:
453         continue;
454     }
455
456     for(PortVectorIterator it = m_Ports.begin();
457         it != m_Ports.end();
458         ++it )
459     {
460         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
461         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
462                     "idx %u: looking at port %s at position %u, location %u\n",
463                     idx, (*it)->getName().c_str(), pinfo->getPosition(), pinfo->getLocation());
464         if ((*it)->getPortType() == Port::E_Midi) {
465             struct _MIDI_port_cache p;
466             p.port = dynamic_cast<AmdtpMidiPort *>(*it);
467             if(p.port == NULL) {
468                 debugError("Port is not an AmdtpMidiPort!\n");
469                 return false;
470             }
471             p.position = pinfo->getPosition();
472             p.location = pinfo->getLocation();
473             p.buffer = NULL; // to be filled by updatePortCache
474             #ifdef DEBUG
475             p.buffer_size = (*it)->getBufferSize();
476             #endif
477
478             m_midi_ports.push_back(p);
479             debugOutput(DEBUG_LEVEL_VERBOSE,
480                         "Cached port %s at position %u, location %u\n",
481                         p.port->getName().c_str(), p.position, p.location);
482         }
483     }
484
485     return true;
486 }
487
488 void
489 AmdtpReceiveStreamProcessor::updatePortCache() {
490     unsigned int idx;
491     for (idx = 0; idx < m_nb_audio_ports; idx++) {
492         struct _MBLA_port_cache& p = m_audio_ports.at(idx);
493         AmdtpAudioPort *port = p.port;
494         p.buffer = port->getBufferAddress();
495         p.enabled = !port->isDisabled();
496     }
497     for (idx = 0; idx < m_nb_midi_ports; idx++) {
498         struct _MIDI_port_cache& p = m_midi_ports.at(idx);
499         AmdtpMidiPort *port = p.port;
500         p.buffer = port->getBufferAddress();
501         p.enabled = !port->isDisabled();
502     }
503 }
504
505 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.