root/branches/api-cleanup/src/libstreaming/amdtp/AmdtpReceiveStreamProcessor.cpp

Revision 811, 14.5 kB (checked in by ppalmers, 16 years ago)

fixes for introduced bugs

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         return eCRV_OK;
170     } else {
171         return eCRV_XRun;
172     }
173 }
174
175 /***********************************************
176  * Encoding/Decoding API                       *
177  ***********************************************/
178 /**
179  * @brief write received events to the stream ringbuffers.
180  */
181 bool AmdtpReceiveStreamProcessor::processReadBlock(char *data,
182                        unsigned int nevents, unsigned int offset)
183 {
184     debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "(%p)->processReadBlock(%u, %u)\n",this,nevents,offset);
185
186     bool no_problem=true;
187
188     for ( PortVectorIterator it = m_Ports.begin();
189           it != m_Ports.end();
190           ++it )
191     {
192         if((*it)->isDisabled()) {continue;};
193
194         //FIXME: make this into a static_cast when not DEBUG?
195
196         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
197         assert(pinfo); // this should not fail!!
198
199         switch(pinfo->getFormat()) {
200         case AmdtpPortInfo::E_MBLA:
201             if(decodeMBLAEventsToPort(static_cast<AmdtpAudioPort *>(*it), (quadlet_t *)data, offset, nevents)) {
202                 debugWarning("Could not decode packet MBLA to port %s",(*it)->getName().c_str());
203                 no_problem=false;
204             }
205             break;
206         case AmdtpPortInfo::E_SPDIF: // still unimplemented
207             break;
208         case AmdtpPortInfo::E_Midi:
209             if(decodeMidiEventsToPort(static_cast<AmdtpMidiPort *>(*it), (quadlet_t *)data, offset, nevents)) {
210                 debugWarning("Could not decode packet Midi to port %s",(*it)->getName().c_str());
211                 no_problem=false;
212             }
213             break;
214         default: // ignore
215             break;
216         }
217     }
218     return no_problem;
219 }
220
221 #if USE_SSE
222 #error broken
223 typedef float v4sf __attribute__ ((vector_size (16)));
224 typedef int v4si __attribute__ ((vector_size (16)));
225 typedef int v2si __attribute__ ((vector_size (8)));
226
227 int
228 AmdtpReceiveStreamProcessor::decodeMBLAEventsToPort(
229                        AmdtpAudioPort *p, quadlet_t *data,
230                        unsigned int offset, unsigned int nevents)
231 {
232     unsigned int j=0;
233     quadlet_t *target_event;
234
235     target_event=(quadlet_t *)(data + p->getPosition());
236
237     static const float multiplier = 1.0f / (float)(0x7FFFFF);
238     static const float sse_multiplier[4] __attribute__((aligned(16))) = {
239             1.0f / (float)(0x7FFFFF),
240             1.0f / (float)(0x7FFFFF),
241             1.0f / (float)(0x7FFFFF),
242             1.0f / (float)(0x7FFFFF)
243     };
244     unsigned int tmp[4];
245
246     switch(p->getDataType()) {
247         default:
248             debugError("bad type: %d\n", p->getDataType());
249             return -1;
250         case Port::E_Int24:
251             {
252                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
253
254                 assert(nevents + offset <= p->getBufferSize());
255
256                 buffer+=offset;
257
258                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
259                     *(buffer)=(ntohl((*target_event) ) & 0x00FFFFFF);
260                     buffer++;
261                     target_event+=m_dimension;
262                 }
263             }
264             break;
265         case Port::E_Float:
266             {
267                 float *buffer=(float *)(p->getBufferAddress());
268
269                 assert(nevents + offset <= p->getBufferSize());
270
271                 buffer += offset;
272                 j = 0;
273                 if(nevents > 3) {
274                     for(j = 0; j < nevents-3; j += 4) {
275                         tmp[0] = ntohl(*target_event);
276                         target_event += m_dimension;
277                         tmp[1] = ntohl(*target_event);
278                         target_event += m_dimension;
279                         tmp[2] = ntohl(*target_event);
280                         target_event += m_dimension;
281                         tmp[3] = ntohl(*target_event);
282                         target_event += m_dimension;
283                         asm("pslld $8, %[in2]\n\t" // sign extend 24th bit
284                                 "pslld $8, %[in1]\n\t"
285                                 "psrad $8, %[in2]\n\t"
286                                 "psrad $8, %[in1]\n\t"
287                                 "cvtpi2ps %[in2], %%xmm0\n\t"
288                                 "movlhps %%xmm0, %%xmm0\n\t"
289                                 "cvtpi2ps %[in1], %%xmm0\n\t"
290                                 "mulps %[ssemult], %%xmm0\n\t"
291                                 "movups %%xmm0, %[floatbuff]"
292                             : [floatbuff] "=m" (*(v4sf*)buffer)
293                             : [in1] "y" (*(v2si*)tmp),
294                         [in2] "y" (*(v2si*)(tmp+2)),
295                         [ssemult] "x" (*(v4sf*)sse_multiplier)
296                             : "xmm0");
297                         buffer += 4;
298                     }
299                 }
300                 for(; j < nevents; ++j) { // decode max nsamples
301                     unsigned int v = ntohl(*target_event) & 0x00FFFFFF;
302                     // sign-extend highest bit of 24-bit int
303                     int tmp = (int)(v << 8) / 256;
304                     *buffer = tmp * multiplier;
305
306                     buffer++;
307                     target_event += m_dimension;
308                 }
309                 asm volatile("emms");
310                 break;
311             }
312             break;
313     }
314
315     return 0;
316 }
317
318 int
319 AmdtpReceiveStreamProcessor::decodeMidiEventsToPort(
320                        AmdtpMidiPort *p, quadlet_t *data,
321                        unsigned int offset, unsigned int nevents)
322 {
323     #warning implement
324 }
325
326 #else
327
328 int
329 AmdtpReceiveStreamProcessor::decodeMBLAEventsToPort(
330                        AmdtpAudioPort *p, quadlet_t *data,
331                        unsigned int offset, unsigned int nevents)
332 {
333     unsigned int j=0;
334     quadlet_t *target_event;
335
336     target_event=(quadlet_t *)(data + p->getPosition());
337
338     switch(p->getDataType()) {
339         default:
340             debugError("bad type: %d\n", p->getDataType());
341             return -1;
342         case Port::E_Int24:
343             {
344                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
345
346                 assert(nevents + offset <= p->getBufferSize());
347
348                 buffer+=offset;
349
350                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
351                     *(buffer)=(ntohl((*target_event) ) & 0x00FFFFFF);
352                     buffer++;
353                     target_event+=m_dimension;
354                 }
355             }
356             break;
357         case Port::E_Float:
358             {
359                 const float multiplier = 1.0f / (float)(0x7FFFFF);
360                 float *buffer=(float *)(p->getBufferAddress());
361
362                 assert(nevents + offset <= p->getBufferSize());
363
364                 buffer+=offset;
365
366                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
367
368                     unsigned int v = ntohl(*target_event) & 0x00FFFFFF;
369                     // sign-extend highest bit of 24-bit int
370                     int tmp = (int)(v << 8) / 256;
371
372                     *buffer = tmp * multiplier;
373
374                     buffer++;
375                     target_event+=m_dimension;
376                 }
377             }
378             break;
379     }
380
381     return 0;
382 }
383
384 int
385 AmdtpReceiveStreamProcessor::decodeMidiEventsToPort(
386                        AmdtpMidiPort *p, quadlet_t *data,
387                        unsigned int offset, unsigned int nevents)
388 {
389     unsigned int j=0;
390     quadlet_t *target_event;
391     quadlet_t sample_int;
392     unsigned int position = p->getPosition();
393     unsigned int location = p->getLocation();
394
395     switch(p->getDataType()) {
396         default:
397             debugError("bad type: %d\n", p->getDataType());
398             return -1;
399         case Port::E_MidiEvent:
400             {
401                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
402
403                 assert(nevents + offset <= p->getBufferSize());
404
405                 buffer+=offset;
406
407                 // clear
408                 memset(buffer, 0, nevents * 4);
409
410                 // assumes that dbc%8 == 0, which is always true if data points to the
411                 // start of a packet in blocking mode
412                 // midi events that belong to the same time mpx-ed block should all be
413                 // timed at the SYT timestamp of the packet. This basically means that they
414                 // all correspond to the first audio frame in the packet.
415                 for(j = location; j < nevents; j += 8) {
416                     target_event=(quadlet_t *)(data + ((j * m_dimension) + position));
417                     sample_int=ntohl(*target_event);
418                     // FIXME: this assumes that 2X and 3X speed isn't used,
419                     // because only the 1X slot is put into the ringbuffer
420                     if(IEC61883_AM824_GET_LABEL(sample_int) != IEC61883_AM824_LABEL_MIDI_NO_DATA) {
421                         sample_int=(sample_int >> 16) & 0x000000FF;
422                         sample_int |= 0x01000000; // flag that there is a midi event present
423                         *buffer = sample_int;
424                     }
425                     buffer += 8; // skip 8 frames
426                 }
427             }
428             break;
429     }
430
431     return 0;
432 }
433
434 #endif
435 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.