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

Revision 790, 14.3 kB (checked in by ppalmers, 14 years ago)

add debugging code

Line 
1 /*
2  * Copyright (C) 2005-2007 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 3 of the License, or
12  * (at your option) any later version.
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         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"STMP: %lluticks | syt_interval=%d, tpf=%f\n",
164             m_last_timestamp, m_syt_interval, getTicksPerFrame());
165     }
166     #endif
167
168     if(m_data_buffer->writeFrames(nevents, (char *)(data+8), m_last_timestamp)) {
169         // process all ports that should be handled on a per-packet base
170         // this is MIDI for AMDTP (due to the need of DBC)
171         if(isRunning()) {
172             if (!decodePacketPorts((quadlet_t *)(data+8), nevents, packet->dbc)) {
173                 debugWarning("Problem decoding Packet Ports\n");
174             }
175         }
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     debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "(%p)->processReadBlock(%u, %u)\n",this,nevents,offset);
192
193     bool no_problem=true;
194
195     for ( PortVectorIterator it = m_PeriodPorts.begin();
196           it != m_PeriodPorts.end();
197           ++it )
198     {
199         if((*it)->isDisabled()) {continue;};
200
201         //FIXME: make this into a static_cast when not DEBUG?
202
203         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
204         assert(pinfo); // this should not fail!!
205
206         switch(pinfo->getFormat()) {
207         case AmdtpPortInfo::E_MBLA:
208             if(decodeMBLAEventsToPort(static_cast<AmdtpAudioPort *>(*it), (quadlet_t *)data, offset, nevents)) {
209                 debugWarning("Could not decode packet MBLA to port %s",(*it)->getName().c_str());
210                 no_problem=false;
211             }
212             break;
213         case AmdtpPortInfo::E_SPDIF: // still unimplemented
214             break;
215     /* for this processor, midi is a packet based port
216         case AmdtpPortInfo::E_Midi:
217             break;*/
218         default: // ignore
219             break;
220         }
221     }
222     return no_problem;
223 }
224
225 /**
226  * @brief decode a packet for the packet-based ports
227  *
228  * @param data Packet data
229  * @param nevents number of events in data (including events of other ports & port types)
230  * @param dbc DataBlockCount value for this packet
231  * @return true if all successfull
232  */
233 bool AmdtpReceiveStreamProcessor::decodePacketPorts(quadlet_t *data, unsigned int nevents, unsigned int dbc)
234 {
235     bool ok=true;
236
237     quadlet_t *target_event=NULL;
238     unsigned int j;
239
240     for ( PortVectorIterator it = m_PacketPorts.begin();
241           it != m_PacketPorts.end();
242           ++it )
243     {
244
245 #ifdef DEBUG
246         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
247         assert(pinfo); // this should not fail!!
248
249         // the only packet type of events for AMDTP is MIDI in mbla
250         assert(pinfo->getFormat()==AmdtpPortInfo::E_Midi);
251 #endif
252         AmdtpMidiPort *mp=static_cast<AmdtpMidiPort *>(*it);
253
254         // we decode this directly (no function call) due to the high frequency
255         /* idea:
256         spec says: current_midi_port=(dbc+j)%8;
257         => if we start at (dbc+stream->location-1)%8,
258         we'll start at the right event for the midi port.
259         => if we increment j with 8, we stay at the right event.
260         */
261         // FIXME: as we know in advance how big a packet is (syt_interval) we can
262         //        predict how much loops will be present here
263         for(j = (dbc & 0x07)+mp->getLocation(); j < nevents; j += 8) {
264             target_event=(quadlet_t *)(data + ((j * m_dimension) + mp->getPosition()));
265             quadlet_t sample_int=ntohl(*target_event);
266             // FIXME: this assumes that 2X and 3X speed isn't used,
267             // because only the 1X slot is put into the ringbuffer
268             if(IEC61883_AM824_GET_LABEL(sample_int) != IEC61883_AM824_LABEL_MIDI_NO_DATA) {
269                 sample_int=(sample_int >> 16) & 0x000000FF;
270                 if(!mp->writeEvent(&sample_int)) {
271                     debugWarning("Packet port events lost\n");
272                     ok=false;
273                 }
274             }
275         }
276
277     }
278
279     return ok;
280 }
281
282 #if USE_SSE
283 typedef float v4sf __attribute__ ((vector_size (16)));
284 typedef int v4si __attribute__ ((vector_size (16)));
285 typedef int v2si __attribute__ ((vector_size (8)));
286
287 int
288 AmdtpReceiveStreamProcessor::decodeMBLAEventsToPort(
289                        AmdtpAudioPort *p, quadlet_t *data,
290                        unsigned int offset, unsigned int nevents)
291 {
292     unsigned int j=0;
293     quadlet_t *target_event;
294
295     target_event=(quadlet_t *)(data + p->getPosition());
296
297     static const float multiplier = 1.0f / (float)(0x7FFFFF);
298     static const float sse_multiplier[4] __attribute__((aligned(16))) = {
299             1.0f / (float)(0x7FFFFF),
300             1.0f / (float)(0x7FFFFF),
301             1.0f / (float)(0x7FFFFF),
302             1.0f / (float)(0x7FFFFF)
303     };
304     unsigned int tmp[4];
305
306     switch(p->getDataType()) {
307         default:
308         case Port::E_Int24:
309             {
310                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
311
312                 assert(nevents + offset <= p->getBufferSize());
313
314                 buffer+=offset;
315
316                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
317                     *(buffer)=(ntohl((*target_event) ) & 0x00FFFFFF);
318                     buffer++;
319                     target_event+=m_dimension;
320                 }
321             }
322             break;
323         case Port::E_Float:
324             {
325                 float *buffer=(float *)(p->getBufferAddress());
326
327                 assert(nevents + offset <= p->getBufferSize());
328
329                 buffer += offset;
330                 j = 0;
331                 if(nevents > 3) {
332                     for(j = 0; j < nevents-3; j += 4) {
333                         tmp[0] = ntohl(*target_event);
334                         target_event += m_dimension;
335                         tmp[1] = ntohl(*target_event);
336                         target_event += m_dimension;
337                         tmp[2] = ntohl(*target_event);
338                         target_event += m_dimension;
339                         tmp[3] = ntohl(*target_event);
340                         target_event += m_dimension;
341                         asm("pslld $8, %[in2]\n\t" // sign extend 24th bit
342                                 "pslld $8, %[in1]\n\t"
343                                 "psrad $8, %[in2]\n\t"
344                                 "psrad $8, %[in1]\n\t"
345                                 "cvtpi2ps %[in2], %%xmm0\n\t"
346                                 "movlhps %%xmm0, %%xmm0\n\t"
347                                 "cvtpi2ps %[in1], %%xmm0\n\t"
348                                 "mulps %[ssemult], %%xmm0\n\t"
349                                 "movups %%xmm0, %[floatbuff]"
350                             : [floatbuff] "=m" (*(v4sf*)buffer)
351                             : [in1] "y" (*(v2si*)tmp),
352                         [in2] "y" (*(v2si*)(tmp+2)),
353                         [ssemult] "x" (*(v4sf*)sse_multiplier)
354                             : "xmm0");
355                         buffer += 4;
356                     }
357                 }
358                 for(; j < nevents; ++j) { // decode max nsamples
359                     unsigned int v = ntohl(*target_event) & 0x00FFFFFF;
360                     // sign-extend highest bit of 24-bit int
361                     int tmp = (int)(v << 8) / 256;
362                     *buffer = tmp * multiplier;
363
364                     buffer++;
365                     target_event += m_dimension;
366                 }
367                 asm volatile("emms");
368                 break;
369             }
370             break;
371     }
372
373     return 0;
374 }
375
376 #else
377
378 int
379 AmdtpReceiveStreamProcessor::decodeMBLAEventsToPort(
380                        AmdtpAudioPort *p, quadlet_t *data,
381                        unsigned int offset, unsigned int nevents)
382 {
383     unsigned int j=0;
384     quadlet_t *target_event;
385
386     target_event=(quadlet_t *)(data + p->getPosition());
387
388     switch(p->getDataType()) {
389         default:
390         case Port::E_Int24:
391             {
392                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
393
394                 assert(nevents + offset <= p->getBufferSize());
395
396                 buffer+=offset;
397
398                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
399                     *(buffer)=(ntohl((*target_event) ) & 0x00FFFFFF);
400                     buffer++;
401                     target_event+=m_dimension;
402                 }
403             }
404             break;
405         case Port::E_Float:
406             {
407                 const float multiplier = 1.0f / (float)(0x7FFFFF);
408                 float *buffer=(float *)(p->getBufferAddress());
409
410                 assert(nevents + offset <= p->getBufferSize());
411
412                 buffer+=offset;
413
414                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
415
416                     unsigned int v = ntohl(*target_event) & 0x00FFFFFF;
417                     // sign-extend highest bit of 24-bit int
418                     int tmp = (int)(v << 8) / 256;
419
420                     *buffer = tmp * multiplier;
421
422                     buffer++;
423                     target_event+=m_dimension;
424                 }
425             }
426             break;
427     }
428
429     return 0;
430 }
431 #endif
432 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.