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

Revision 866, 14.3 kB (checked in by ppalmers, 15 years ago)

- weed out some unused functions
- introduce 'debugOutputExtreme' allowing to disable debug statements in the speed-sensitive sections. This should reduce the cpu load on a 'normal' debug build significantly.

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 <netinet/in.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 {}
45
46 unsigned int
47 AmdtpReceiveStreamProcessor::getSytInterval() {
48     switch (m_StreamProcessorManager.getNominalRate()) {
49         case 32000:
50         case 44100:
51         case 48000:
52             return 8;
53         case 88200:
54         case 96000:
55             return 16;
56         case 176400:
57         case 192000:
58             return 32;
59         default:
60             debugError("Unsupported rate: %d\n", m_StreamProcessorManager.getNominalRate());
61             return 0;
62     }
63 }
64
65 bool AmdtpReceiveStreamProcessor::prepareChild() {
66     debugOutput( DEBUG_LEVEL_VERBOSE, "Preparing (%p)...\n", this);
67     m_syt_interval = getSytInterval();
68     return true;
69 }
70
71
72 /**
73  * Processes packet header to extract timestamps and so on
74  * @param data
75  * @param length
76  * @param channel
77  * @param tag
78  * @param sy
79  * @param cycle
80  * @param dropped
81  * @return
82  */
83 enum StreamProcessor::eChildReturnValue
84 AmdtpReceiveStreamProcessor::processPacketHeader(unsigned char *data, unsigned int length,
85                   unsigned char channel, unsigned char tag, unsigned char sy,
86                   unsigned int cycle, unsigned int dropped)
87 {
88     #ifdef DEBUG
89     static uint32_t now_prev=0;
90     static uint64_t now_prev_ticks=0;
91     #endif
92
93     struct iec61883_packet *packet = (struct iec61883_packet *) data;
94     assert(packet);
95     bool ok = (packet->syt != 0xFFFF) &&
96                   (packet->fdf != 0xFF) &&
97                   (packet->fmt == 0x10) &&
98                   (packet->dbs > 0) &&
99                   (length >= 2*sizeof(quadlet_t));
100     if(ok) {
101         uint32_t now = m_1394service.getCycleTimer();
102
103         #ifdef DEBUG
104         uint64_t now_ticks = CYCLE_TIMER_TO_TICKS(now);
105
106         if (diffTicks(now_ticks, now_prev_ticks) < 0) {
107             debugWarning("non-monotonic CTR on cycle %04u: %llu -> %llu\n", cycle, now_prev_ticks, now_ticks);
108             debugWarning("                               : %08X -> %08X\n", now_prev, now);
109             debugOutput ( DEBUG_LEVEL_VERBOSE,
110                         " current: %011llu (%03us %04ucy %04uticks)\n",
111                         now,
112                         (unsigned int)TICKS_TO_SECS( now ),
113                         (unsigned int)TICKS_TO_CYCLES( now ),
114                         (unsigned int)TICKS_TO_OFFSET( now ) );
115             debugOutput ( DEBUG_LEVEL_VERBOSE,
116                         " prev   : %011llu (%03us %04ucy %04uticks)\n",
117                         now_prev,
118                         (unsigned int)TICKS_TO_SECS( now_prev ),
119                         (unsigned int)TICKS_TO_CYCLES( now_prev ),
120                         (unsigned int)TICKS_TO_OFFSET( now_prev ) );
121         }
122         now_prev = now;
123         now_prev_ticks=now_ticks;
124         #endif
125
126         //=> convert the SYT to a full timestamp in ticks
127         m_last_timestamp = sytRecvToFullTicks((uint32_t)ntohs(packet->syt),
128                                               cycle, now);
129     }
130     return (ok ? eCRV_OK : eCRV_Invalid );
131 }
132
133 /**
134  * extract the data from the packet
135  * @pre the IEC61883 packet is valid according to isValidPacket
136  * @param data
137  * @param length
138  * @param channel
139  * @param tag
140  * @param sy
141  * @param cycle
142  * @param dropped
143  * @return
144  */
145 enum StreamProcessor::eChildReturnValue
146 AmdtpReceiveStreamProcessor::processPacketData(unsigned char *data, unsigned int length,
147                   unsigned char channel, unsigned char tag, unsigned char sy,
148                   unsigned int cycle, unsigned int dropped_cycles) {
149     struct iec61883_packet *packet = (struct iec61883_packet *) data;
150     assert(packet);
151
152     unsigned int nevents=((length / sizeof (quadlet_t)) - 2)/packet->dbs;
153
154     // we have to keep in mind that there are also
155     // some packets buffered by the ISO layer,
156     // at most x=m_handler->getWakeupInterval()
157     // these contain at most x*syt_interval
158     // frames, meaning that we might receive
159     // this packet x*syt_interval*ticks_per_frame
160     // later than expected (the real receive time)
161     #ifdef DEBUG
162     if(isRunning()) {
163         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
164                            "STMP: %lluticks | syt_interval=%d, tpf=%f\n",
165                            m_last_timestamp, m_syt_interval, getTicksPerFrame());
166     }
167     #endif
168
169     if(m_data_buffer->writeFrames(nevents, (char *)(data+8), m_last_timestamp)) {
170         return eCRV_OK;
171     } else {
172         return eCRV_XRun;
173     }
174 }
175
176 /***********************************************
177  * Encoding/Decoding API                       *
178  ***********************************************/
179 /**
180  * @brief write received events to the stream ringbuffers.
181  */
182 bool AmdtpReceiveStreamProcessor::processReadBlock(char *data,
183                        unsigned int nevents, unsigned int offset)
184 {
185     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE,
186                         "(%p)->processReadBlock(%u, %u)\n",
187                         this,nevents,offset);
188
189     bool no_problem=true;
190
191     for ( PortVectorIterator it = m_Ports.begin();
192           it != m_Ports.end();
193           ++it )
194     {
195         if((*it)->isDisabled()) {continue;};
196
197         //FIXME: make this into a static_cast when not DEBUG?
198
199         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
200         assert(pinfo); // this should not fail!!
201
202         switch(pinfo->getFormat()) {
203         case AmdtpPortInfo::E_MBLA:
204             if(decodeMBLAEventsToPort(static_cast<AmdtpAudioPort *>(*it), (quadlet_t *)data, offset, nevents)) {
205                 debugWarning("Could not decode packet MBLA to port %s",(*it)->getName().c_str());
206                 no_problem=false;
207             }
208             break;
209         case AmdtpPortInfo::E_SPDIF: // still unimplemented
210             break;
211         case AmdtpPortInfo::E_Midi:
212             if(decodeMidiEventsToPort(static_cast<AmdtpMidiPort *>(*it), (quadlet_t *)data, offset, nevents)) {
213                 debugWarning("Could not decode packet Midi to port %s",(*it)->getName().c_str());
214                 no_problem=false;
215             }
216             break;
217         default: // ignore
218             break;
219         }
220     }
221     return no_problem;
222 }
223
224 #if USE_SSE
225 #error broken
226 typedef float v4sf __attribute__ ((vector_size (16)));
227 typedef int v4si __attribute__ ((vector_size (16)));
228 typedef int v2si __attribute__ ((vector_size (8)));
229
230 int
231 AmdtpReceiveStreamProcessor::decodeMBLAEventsToPort(
232                        AmdtpAudioPort *p, quadlet_t *data,
233                        unsigned int offset, unsigned int nevents)
234 {
235     unsigned int j=0;
236     quadlet_t *target_event;
237
238     target_event=(quadlet_t *)(data + p->getPosition());
239
240     static const float multiplier = 1.0f / (float)(0x7FFFFF);
241     static const float sse_multiplier[4] __attribute__((aligned(16))) = {
242             1.0f / (float)(0x7FFFFF),
243             1.0f / (float)(0x7FFFFF),
244             1.0f / (float)(0x7FFFFF),
245             1.0f / (float)(0x7FFFFF)
246     };
247     unsigned int tmp[4];
248
249     switch(p->getDataType()) {
250         default:
251             debugError("bad type: %d\n", p->getDataType());
252             return -1;
253         case Port::E_Int24:
254             {
255                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
256
257                 assert(nevents + offset <= p->getBufferSize());
258
259                 buffer+=offset;
260
261                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
262                     *(buffer)=(ntohl((*target_event) ) & 0x00FFFFFF);
263                     buffer++;
264                     target_event+=m_dimension;
265                 }
266             }
267             break;
268         case Port::E_Float:
269             {
270                 float *buffer=(float *)(p->getBufferAddress());
271
272                 assert(nevents + offset <= p->getBufferSize());
273
274                 buffer += offset;
275                 j = 0;
276                 if(nevents > 3) {
277                     for(j = 0; j < nevents-3; j += 4) {
278                         tmp[0] = ntohl(*target_event);
279                         target_event += m_dimension;
280                         tmp[1] = ntohl(*target_event);
281                         target_event += m_dimension;
282                         tmp[2] = ntohl(*target_event);
283                         target_event += m_dimension;
284                         tmp[3] = ntohl(*target_event);
285                         target_event += m_dimension;
286                         asm("pslld $8, %[in2]\n\t" // sign extend 24th bit
287                                 "pslld $8, %[in1]\n\t"
288                                 "psrad $8, %[in2]\n\t"
289                                 "psrad $8, %[in1]\n\t"
290                                 "cvtpi2ps %[in2], %%xmm0\n\t"
291                                 "movlhps %%xmm0, %%xmm0\n\t"
292                                 "cvtpi2ps %[in1], %%xmm0\n\t"
293                                 "mulps %[ssemult], %%xmm0\n\t"
294                                 "movups %%xmm0, %[floatbuff]"
295                             : [floatbuff] "=m" (*(v4sf*)buffer)
296                             : [in1] "y" (*(v2si*)tmp),
297                         [in2] "y" (*(v2si*)(tmp+2)),
298                         [ssemult] "x" (*(v4sf*)sse_multiplier)
299                             : "xmm0");
300                         buffer += 4;
301                     }
302                 }
303                 for(; j < nevents; ++j) { // decode max nsamples
304                     unsigned int v = ntohl(*target_event) & 0x00FFFFFF;
305                     // sign-extend highest bit of 24-bit int
306                     int tmp = (int)(v << 8) / 256;
307                     *buffer = tmp * multiplier;
308
309                     buffer++;
310                     target_event += m_dimension;
311                 }
312                 asm volatile("emms");
313                 break;
314             }
315             break;
316     }
317
318     return 0;
319 }
320
321 int
322 AmdtpReceiveStreamProcessor::decodeMidiEventsToPort(
323                        AmdtpMidiPort *p, quadlet_t *data,
324                        unsigned int offset, unsigned int nevents)
325 {
326     #warning implement
327 }
328
329 #else
330
331 int
332 AmdtpReceiveStreamProcessor::decodeMBLAEventsToPort(
333                        AmdtpAudioPort *p, quadlet_t *data,
334                        unsigned int offset, unsigned int nevents)
335 {
336     unsigned int j=0;
337     quadlet_t *target_event;
338
339     target_event=(quadlet_t *)(data + p->getPosition());
340
341     switch(m_StreamProcessorManager.getAudioDataType()) {
342         default:
343             debugError("bad type: %d\n", m_StreamProcessorManager.getAudioDataType());
344             return -1;
345         case StreamProcessorManager::eADT_Int24:
346             {
347                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
348
349                 assert(nevents + offset <= p->getBufferSize());
350
351                 buffer+=offset;
352
353                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
354                     *(buffer)=(ntohl((*target_event) ) & 0x00FFFFFF);
355                     buffer++;
356                     target_event+=m_dimension;
357                 }
358             }
359             break;
360         case StreamProcessorManager::eADT_Float:
361             {
362                 const float multiplier = 1.0f / (float)(0x7FFFFF);
363                 float *buffer=(float *)(p->getBufferAddress());
364
365                 assert(nevents + offset <= p->getBufferSize());
366
367                 buffer+=offset;
368
369                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
370
371                     unsigned int v = ntohl(*target_event) & 0x00FFFFFF;
372                     // sign-extend highest bit of 24-bit int
373                     int tmp = (int)(v << 8) / 256;
374
375                     *buffer = tmp * multiplier;
376
377                     buffer++;
378                     target_event+=m_dimension;
379                 }
380             }
381             break;
382     }
383
384     return 0;
385 }
386
387 int
388 AmdtpReceiveStreamProcessor::decodeMidiEventsToPort(
389                        AmdtpMidiPort *p, quadlet_t *data,
390                        unsigned int offset, unsigned int nevents)
391 {
392     unsigned int j=0;
393     quadlet_t *target_event;
394     quadlet_t sample_int;
395     unsigned int position = p->getPosition();
396     unsigned int location = p->getLocation();
397
398     quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
399
400     assert(nevents + offset <= p->getBufferSize());
401
402     buffer+=offset;
403
404     // clear
405     memset(buffer, 0, nevents * 4);
406
407     // assumes that dbc%8 == 0, which is always true if data points to the
408     // start of a packet in blocking mode
409     // midi events that belong to the same time mpx-ed block should all be
410     // timed at the SYT timestamp of the packet. This basically means that they
411     // all correspond to the first audio frame in the packet.
412     for(j = location; j < nevents; j += 8) {
413         target_event=(quadlet_t *)(data + ((j * m_dimension) + position));
414         sample_int=ntohl(*target_event);
415         // FIXME: this assumes that 2X and 3X speed isn't used,
416         // because only the 1X slot is put into the ringbuffer
417         if(IEC61883_AM824_GET_LABEL(sample_int) != IEC61883_AM824_LABEL_MIDI_NO_DATA) {
418             sample_int=(sample_int >> 16) & 0x000000FF;
419             sample_int |= 0x01000000; // flag that there is a midi event present
420             *buffer = sample_int;
421             debugOutput(DEBUG_LEVEL_VERBOSE, "Received midi byte %08X on port %p index %d\n", sample_int, p, j-location);
422         }
423         buffer += 8; // skip 8 frames
424     }
425
426     return 0;
427 }
428
429 #endif
430 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.