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

Revision 796, 27.3 kB (checked in by ppalmers, 13 years ago)

- move #define constants to config.h.in
- switch receive handler over to packet-per-buffer mode to improve latency performance

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
24 #include "config.h"
25 #include "AmdtpTransmitStreamProcessor.h"
26 #include "AmdtpPort.h"
27 #include "../StreamProcessorManager.h"
28 #include "devicemanager.h"
29
30 #include "libutil/Time.h"
31
32 #include "libieee1394/ieee1394service.h"
33 #include "libieee1394/IsoHandlerManager.h"
34 #include "libieee1394/cycletimer.h"
35
36 #include <netinet/in.h>
37 #include <assert.h>
38
39 namespace Streaming
40 {
41
42 /* transmit */
43 AmdtpTransmitStreamProcessor::AmdtpTransmitStreamProcessor(FFADODevice &parent, int dimension)
44         : StreamProcessor(parent, ePT_Transmit)
45         , m_dimension( dimension )
46         , m_dbc( 0 )
47 {}
48
49 enum StreamProcessor::eChildReturnValue
50 AmdtpTransmitStreamProcessor::generatePacketHeader (
51     unsigned char *data, unsigned int *length,
52     unsigned char *tag, unsigned char *sy,
53     int cycle, unsigned int dropped, unsigned int max_length )
54 {
55     struct iec61883_packet *packet = ( struct iec61883_packet * ) data;
56     /* Our node ID can change after a bus reset, so it is best to fetch
57     * our node ID for each packet. */
58     packet->sid = m_1394service.getLocalNodeId() & 0x3f;
59
60     packet->dbs = m_dimension;
61     packet->fn = 0;
62     packet->qpc = 0;
63     packet->sph = 0;
64     packet->reserved = 0;
65     packet->dbc = m_dbc;
66     packet->eoh1 = 2;
67     packet->fmt = IEC61883_FMT_AMDTP;
68
69     *tag = IEC61883_TAG_WITH_CIP;
70     *sy = 0;
71
72     signed int fc;
73     uint64_t presentation_time;
74     unsigned int presentation_cycle;
75     int cycles_until_presentation;
76
77     uint64_t transmit_at_time;
78     unsigned int transmit_at_cycle;
79     int cycles_until_transmit;
80
81     // FIXME: should become a define
82     // the absolute minimum number of cycles we want to transmit
83     // a packet ahead of the presentation time. The nominal time
84     // the packet is transmitted ahead of the presentation time is
85     // given by AMDTP_TRANSMIT_TRANSFER_DELAY (in ticks), but in case we
86     // are too late for that, this constant defines how late we can
87     // be.
88     const int min_cycles_before_presentation = 1;
89     // FIXME: should become a define
90     // the absolute maximum number of cycles we want to transmit
91     // a packet ahead of the ideal transmit time. The nominal time
92     // the packet is transmitted ahead of the presentation time is
93     // given by AMDTP_TRANSMIT_TRANSFER_DELAY (in ticks), but we can send
94     // packets early if we want to. (not completely according to spec)
95     const int max_cycles_to_transmit_early = 2;
96
97     debugOutput ( DEBUG_LEVEL_ULTRA_VERBOSE, "Try for cycle %d\n", cycle );
98     // check whether the packet buffer has packets for us to send.
99     // the base timestamp is the one of the next sample in the buffer
100     ffado_timestamp_t ts_head_tmp;
101     m_data_buffer->getBufferHeadTimestamp ( &ts_head_tmp, &fc ); // thread safe
102
103     // the timestamp gives us the time at which we want the sample block
104     // to be output by the device
105     presentation_time = ( uint64_t ) ts_head_tmp;
106     m_last_timestamp = presentation_time;
107
108     // now we calculate the time when we have to transmit the sample block
109     transmit_at_time = substractTicks ( presentation_time, AMDTP_TRANSMIT_TRANSFER_DELAY );
110
111     // calculate the cycle this block should be presented in
112     // (this is just a virtual calculation since at that time it should
113     //  already be in the device's buffer)
114     presentation_cycle = ( unsigned int ) ( TICKS_TO_CYCLES ( presentation_time ) );
115
116     // calculate the cycle this block should be transmitted in
117     transmit_at_cycle = ( unsigned int ) ( TICKS_TO_CYCLES ( transmit_at_time ) );
118
119     // we can check whether this cycle is within the 'window' we have
120     // to send this packet.
121     // first calculate the number of cycles left before presentation time
122     cycles_until_presentation = diffCycles ( presentation_cycle, cycle );
123
124     // we can check whether this cycle is within the 'window' we have
125     // to send this packet.
126     // first calculate the number of cycles left before presentation time
127     cycles_until_transmit = diffCycles ( transmit_at_cycle, cycle );
128
129     if (dropped) {
130         debugOutput ( DEBUG_LEVEL_VERBOSE,
131                     "Gen HDR: CY=%04u, TC=%04u, CUT=%04d, TST=%011llu (%04u), TSP=%011llu (%04u)\n",
132                     cycle,
133                     transmit_at_cycle, cycles_until_transmit,
134                     transmit_at_time, ( unsigned int ) TICKS_TO_CYCLES ( transmit_at_time ),
135                     presentation_time, ( unsigned int ) TICKS_TO_CYCLES ( presentation_time ) );
136     }
137     // two different options:
138     // 1) there are not enough frames for one packet
139     //      => determine wether this is a problem, since we might still
140     //         have some time to send it
141     // 2) there are enough packets
142     //      => determine whether we have to send them in this packet
143     if ( fc < ( signed int ) m_syt_interval )
144     {
145         // not enough frames in the buffer,
146
147         // we can still postpone the queueing of the packets
148         // if we are far enough ahead of the presentation time
149         if ( cycles_until_presentation <= min_cycles_before_presentation )
150         {
151             debugOutput ( DEBUG_LEVEL_VERBOSE,
152                         "Insufficient frames (P): N=%02d, CY=%04u, TC=%04u, CUT=%04d\n",
153                         fc, cycle, transmit_at_cycle, cycles_until_transmit );
154             // we are too late
155             return eCRV_XRun;
156         }
157         else
158         {
159             unsigned int now_cycle = ( unsigned int ) ( TICKS_TO_CYCLES ( m_1394service.getCycleTimerTicks() ) );
160
161             debugOutput ( DEBUG_LEVEL_VERBOSE,
162                         "Insufficient frames (NP): N=%02d, CY=%04u, TC=%04u, CUT=%04d, NOW=%04d\n",
163                         fc, cycle, transmit_at_cycle, cycles_until_transmit, now_cycle );
164             debugWarning("Insufficient frames (NP): N=%02d, CY=%04u, TC=%04u, CUT=%04d, NOW=%04d\n",
165                          fc, cycle, transmit_at_cycle, cycles_until_transmit, now_cycle );
166
167             // there is still time left to send the packet
168             // we want the system to give this packet another go at a later time instant
169             return eCRV_Again; // note that the raw1394 again system doesn't work as expected
170
171             // we could wait here for a certain time before trying again. However, this
172             // is not going to work since we then block the iterator thread, hence also
173             // the receiving code, meaning that we are not processing received packets,
174             // and hence there is no progression in the number of frames available.
175
176             // for example:
177             // SleepRelativeUsec(125); // one cycle
178             // goto try_block_of_frames;
179
180             // or more advanced, calculate how many cycles we are ahead of 'now' and
181             // base the sleep on that.
182
183             // note that this requires that there is one thread for each IsoHandler,
184             // otherwise we're in the deadlock described above.
185         }
186     }
187     else
188     {
189         // there are enough frames, so check the time they are intended for
190         // all frames have a certain 'time window' in which they can be sent
191         // this corresponds to the range of the timestamp mechanism:
192         // we can send a packet 15 cycles in advance of the 'presentation time'
193         // in theory we can send the packet up till one cycle before the presentation time,
194         // however this is not very smart.
195
196         // There are 3 options:
197         // 1) the frame block is too early
198         //      => send an empty packet
199         // 2) the frame block is within the window
200         //      => send it
201         // 3) the frame block is too late
202         //      => discard (and raise xrun?)
203         //         get next block of frames and repeat
204
205         if(cycles_until_transmit < 0)
206         {
207             // we are too late
208             debugOutput(DEBUG_LEVEL_VERBOSE,
209                         "Too late: CY=%04u, TC=%04u, CUT=%04d, TSP=%011llu (%04u)\n",
210                         cycle,
211                         transmit_at_cycle, cycles_until_transmit,
212                         presentation_time, (unsigned int)TICKS_TO_CYCLES(presentation_time) );
213             debugShowBackLogLines(200);
214             flushDebugOutput();
215             // however, if we can send this sufficiently before the presentation
216             // time, it could be harmless.
217             // NOTE: dangerous since the device has no way of reporting that it didn't get
218             //       this packet on time.
219             if(cycles_until_presentation >= min_cycles_before_presentation)
220             {
221                 // we are not that late and can still try to transmit the packet
222                 m_dbc += fillDataPacketHeader(packet, length, m_last_timestamp);
223                 return (fc < (signed)(2*m_syt_interval) ? eCRV_Defer : eCRV_Packet);
224             }
225             else   // definitely too late
226             {
227                 return eCRV_XRun;
228             }
229         }
230         else if(cycles_until_transmit <= max_cycles_to_transmit_early)
231         {
232             // it's time send the packet
233             m_dbc += fillDataPacketHeader(packet, length, m_last_timestamp);
234             return (fc < (signed)(2*m_syt_interval) ? eCRV_Defer : eCRV_Packet);
235         }
236         else
237         {
238             debugOutput ( DEBUG_LEVEL_VERY_VERBOSE,
239                         "Too early: CY=%04u, TC=%04u, CUT=%04d, TST=%011llu (%04u), TSP=%011llu (%04u)\n",
240                         cycle,
241                         transmit_at_cycle, cycles_until_transmit,
242                         transmit_at_time, ( unsigned int ) TICKS_TO_CYCLES ( transmit_at_time ),
243                         presentation_time, ( unsigned int ) TICKS_TO_CYCLES ( presentation_time ) );
244 #ifdef DEBUG
245             if ( cycles_until_transmit > max_cycles_to_transmit_early + 1 )
246             {
247                 debugOutput ( DEBUG_LEVEL_VERY_VERBOSE,
248                             "Way too early: CY=%04u, TC=%04u, CUT=%04d, TST=%011llu (%04u), TSP=%011llu (%04u)\n",
249                             cycle,
250                             transmit_at_cycle, cycles_until_transmit,
251                             transmit_at_time, ( unsigned int ) TICKS_TO_CYCLES ( transmit_at_time ),
252                             presentation_time, ( unsigned int ) TICKS_TO_CYCLES ( presentation_time ) );
253             }
254 #endif
255             // we are too early, send only an empty packet
256             return eCRV_EmptyPacket;
257         }
258     }
259     return eCRV_Invalid;
260 }
261
262 enum StreamProcessor::eChildReturnValue
263 AmdtpTransmitStreamProcessor::generatePacketData (
264     unsigned char *data, unsigned int *length,
265     unsigned char *tag, unsigned char *sy,
266     int cycle, unsigned int dropped, unsigned int max_length )
267 {
268     struct iec61883_packet *packet = ( struct iec61883_packet * ) data;
269     if ( m_data_buffer->readFrames ( m_syt_interval, ( char * ) ( data + 8 ) ) )
270     {
271         // process all ports that should be handled on a per-packet base
272         // this is MIDI for AMDTP (due to the need of DBC)
273         if ( !encodePacketPorts ( ( quadlet_t * ) ( data+8 ), m_syt_interval, packet->dbc ) )
274         {
275             debugWarning ( "Problem encoding Packet Ports\n" );
276         }
277         debugOutput ( DEBUG_LEVEL_ULTRA_VERBOSE, "XMIT DATA (cy %04d): TSP=%011llu (%04u)\n",
278                     cycle, m_last_timestamp, ( unsigned int ) TICKS_TO_CYCLES ( m_last_timestamp ) );
279         return eCRV_OK;
280     }
281     else return eCRV_XRun;
282
283 }
284
285 enum StreamProcessor::eChildReturnValue
286 AmdtpTransmitStreamProcessor::generateSilentPacketHeader (
287     unsigned char *data, unsigned int *length,
288     unsigned char *tag, unsigned char *sy,
289     int cycle, unsigned int dropped, unsigned int max_length )
290 {
291     struct iec61883_packet *packet = ( struct iec61883_packet * ) data;
292     debugOutput ( DEBUG_LEVEL_ULTRA_VERBOSE, "XMIT NONE (cy %04d): CY=%04u, TSP=%011llu (%04u)\n",
293                 cycle, m_last_timestamp, ( unsigned int ) TICKS_TO_CYCLES ( m_last_timestamp ) );
294
295     /* Our node ID can change after a bus reset, so it is best to fetch
296     * our node ID for each packet. */
297     packet->sid = m_1394service.getLocalNodeId() & 0x3f;
298
299     packet->dbs = m_dimension;
300     packet->fn = 0;
301     packet->qpc = 0;
302     packet->sph = 0;
303     packet->reserved = 0;
304     packet->dbc = m_dbc;
305     packet->eoh1 = 2;
306     packet->fmt = IEC61883_FMT_AMDTP;
307
308     *tag = IEC61883_TAG_WITH_CIP;
309     *sy = 0;
310
311     m_dbc += fillNoDataPacketHeader ( packet, length );
312     return eCRV_OK;
313 }
314
315 enum StreamProcessor::eChildReturnValue
316 AmdtpTransmitStreamProcessor::generateSilentPacketData (
317     unsigned char *data, unsigned int *length,
318     unsigned char *tag, unsigned char *sy,
319     int cycle, unsigned int dropped, unsigned int max_length )
320 {
321     return eCRV_OK; // no need to do anything
322 }
323
324 unsigned int AmdtpTransmitStreamProcessor::fillDataPacketHeader (
325     struct iec61883_packet *packet, unsigned int* length,
326     uint32_t ts )
327 {
328
329     packet->fdf = m_fdf;
330
331     // convert the timestamp to SYT format
332     uint16_t timestamp_SYT = TICKS_TO_SYT ( ts );
333     packet->syt = ntohs ( timestamp_SYT );
334
335     *length = m_syt_interval*sizeof ( quadlet_t ) *m_dimension + 8;
336
337     return m_syt_interval;
338 }
339
340 unsigned int AmdtpTransmitStreamProcessor::fillNoDataPacketHeader (
341     struct iec61883_packet *packet, unsigned int* length )
342 {
343
344     // no-data packets have syt=0xFFFF
345     // and have the usual amount of events as dummy data (?)
346     packet->fdf = IEC61883_FDF_NODATA;
347     packet->syt = 0xffff;
348
349     // FIXME: either make this a setting or choose
350     bool send_payload=true;
351     if ( send_payload )
352     {
353         // this means no-data packets with payload (DICE doesn't like that)
354         *length = 2*sizeof ( quadlet_t ) + m_syt_interval * m_dimension * sizeof ( quadlet_t );
355         return m_syt_interval;
356     }
357     else
358     {
359         // dbc is not incremented
360         // this means no-data packets without payload
361         *length = 2*sizeof ( quadlet_t );
362         return 0;
363     }
364 }
365
366 unsigned int
367 AmdtpTransmitStreamProcessor::getSytInterval() {
368     switch (m_StreamProcessorManager.getNominalRate()) {
369         case 32000:
370         case 44100:
371         case 48000:
372             return 8;
373         case 88200:
374         case 96000:
375             return 16;
376         case 176400:
377         case 192000:
378             return 32;
379         default:
380             debugError("Unsupported rate: %d\n", m_StreamProcessorManager.getNominalRate());
381             return 0;
382     }
383 }
384 unsigned int
385 AmdtpTransmitStreamProcessor::getFDF() {
386     switch (m_StreamProcessorManager.getNominalRate()) {
387         case 32000: return IEC61883_FDF_SFC_32KHZ;
388         case 44100: return IEC61883_FDF_SFC_44K1HZ;
389         case 48000: return IEC61883_FDF_SFC_48KHZ;
390         case 88200: return IEC61883_FDF_SFC_88K2HZ;
391         case 96000: return IEC61883_FDF_SFC_96KHZ;
392         case 176400: return IEC61883_FDF_SFC_176K4HZ;
393         case 192000: return IEC61883_FDF_SFC_192KHZ;
394         default:
395             debugError("Unsupported rate: %d\n", m_StreamProcessorManager.getNominalRate());
396             return 0;
397     }
398 }
399
400 bool AmdtpTransmitStreamProcessor::prepareChild()
401 {
402     debugOutput ( DEBUG_LEVEL_VERBOSE, "Preparing (%p)...\n", this );
403     m_syt_interval = getSytInterval();
404     m_fdf = getFDF();
405
406     iec61883_cip_init (
407         &m_cip_status,
408         IEC61883_FMT_AMDTP,
409         m_fdf,
410         m_StreamProcessorManager.getNominalRate(),
411         m_dimension,
412         m_syt_interval );
413
414     for ( PortVectorIterator it = m_Ports.begin();
415             it != m_Ports.end();
416             ++it )
417     {
418         if ( ( *it )->getPortType() == Port::E_Midi )
419         {
420             // we use a timing unit of 10ns
421             // this makes sure that for the max syt interval
422             // we don't have rounding, and keeps the numbers low
423             // we have 1 slot every 8 events
424             // we have syt_interval events per packet
425             // => syt_interval/8 slots per packet
426             // packet rate is 8000pkt/sec => interval=125us
427             // so the slot interval is (1/8000)/(syt_interval/8)
428             // or: 1/(1000 * syt_interval) sec
429             // which is 1e9/(1000*syt_interval) nsec
430             // or 100000/syt_interval 'units'
431             // the event interval is fixed to 320us = 32000 'units'
432             if ( ! ( *it )->useRateControl ( true, ( 100000/m_syt_interval ),32000, false ) )
433             {
434                 debugFatal ( "Could not set signal type to PeriodSignalling" );
435                 return false;
436             }
437             break;
438         }
439     }
440     return true;
441 }
442
443 /*
444 * compose the event streams for the packets from the port buffers
445 */
446 bool AmdtpTransmitStreamProcessor::processWriteBlock ( char *data,
447         unsigned int nevents, unsigned int offset )
448 {
449     bool no_problem = true;
450
451     for ( PortVectorIterator it = m_PeriodPorts.begin();
452           it != m_PeriodPorts.end();
453           ++it )
454     {
455         if ( (*it)->isDisabled() ) { continue; };
456
457         //FIXME: make this into a static_cast when not DEBUG?
458         AmdtpPortInfo *pinfo = dynamic_cast<AmdtpPortInfo *> ( *it );
459         assert ( pinfo ); // this should not fail!!
460
461         switch( pinfo->getFormat() )
462         {
463             case AmdtpPortInfo::E_MBLA:
464                 if( encodePortToMBLAEvents(static_cast<AmdtpAudioPort *>(*it), (quadlet_t *)data, offset, nevents) )
465                 {
466                     debugWarning ( "Could not encode port %s to MBLA events", (*it)->getName().c_str() );
467                     no_problem = false;
468                 }
469                 break;
470             case AmdtpPortInfo::E_SPDIF: // still unimplemented
471                 break;
472             default: // ignore
473                 break;
474         }
475     }
476     return no_problem;
477 }
478
479 bool
480 AmdtpTransmitStreamProcessor::transmitSilenceBlock(
481     char *data, unsigned int nevents, unsigned int offset)
482 {
483     bool no_problem = true;
484     for(PortVectorIterator it = m_PeriodPorts.begin();
485         it != m_PeriodPorts.end();
486         ++it )
487     {
488         //FIXME: make this into a static_cast when not DEBUG?
489         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *>(*it);
490         assert(pinfo); // this should not fail!!
491
492         switch( pinfo->getFormat() )
493         {
494             case AmdtpPortInfo::E_MBLA:
495                 if ( encodeSilencePortToMBLAEvents(static_cast<AmdtpAudioPort *>(*it), (quadlet_t *)data, offset, nevents) )
496                 {
497                     debugWarning("Could not encode port %s to MBLA events", (*it)->getName().c_str());
498                     no_problem = false;
499                 }
500                 break;
501             case AmdtpPortInfo::E_SPDIF: // still unimplemented
502                 break;
503             default: // ignore
504                 break;
505         }
506     }
507     return no_problem;
508 }
509
510 /**
511 * @brief decode a packet for the packet-based ports
512 *
513 * @param data Packet data
514 * @param nevents number of events in data (including events of other ports & port types)
515 * @param dbc DataBlockCount value for this packet
516 * @return true if all successfull
517 */
518 bool AmdtpTransmitStreamProcessor::encodePacketPorts ( quadlet_t *data, unsigned int nevents, unsigned int dbc )
519 {
520     bool ok=true;
521     quadlet_t byte;
522
523     quadlet_t *target_event=NULL;
524     unsigned int j;
525
526     for ( PortVectorIterator it = m_PacketPorts.begin();
527             it != m_PacketPorts.end();
528             ++it )
529     {
530
531 #ifdef DEBUG
532         AmdtpPortInfo *pinfo=dynamic_cast<AmdtpPortInfo *> ( *it );
533         assert ( pinfo ); // this should not fail!!
534
535         // the only packet type of events for AMDTP is MIDI in mbla
536         assert ( pinfo->getFormat() ==AmdtpPortInfo::E_Midi );
537 #endif
538
539         AmdtpMidiPort *mp=static_cast<AmdtpMidiPort *> ( *it );
540
541         // we encode this directly (no function call) due to the high frequency
542         /* idea:
543         spec says: current_midi_port=(dbc+j)%8;
544         => if we start at (dbc+stream->location-1)%8,
545         we'll start at the right event for the midi port.
546         => if we increment j with 8, we stay at the right event.
547         */
548         // FIXME: as we know in advance how big a packet is (syt_interval) we can
549         //        predict how much loops will be present here
550         // first prefill the buffer with NO_DATA's on all time muxed channels
551
552         for ( j = ( dbc & 0x07 ) +mp->getLocation(); j < nevents; j += 8 )
553         {
554
555             quadlet_t tmpval;
556
557             target_event= ( quadlet_t * ) ( data + ( ( j * m_dimension ) + mp->getPosition() ) );
558
559             if ( mp->canRead() )   // we can send a byte
560             {
561                 mp->readEvent ( &byte );
562                 byte &= 0xFF;
563                 tmpval=htonl (
564                         IEC61883_AM824_SET_LABEL ( ( byte ) <<16,
565                                                     IEC61883_AM824_LABEL_MIDI_1X ) );
566
567                 debugOutput ( DEBUG_LEVEL_ULTRA_VERBOSE, "MIDI port %s, pos=%d, loc=%d, dbc=%d, nevents=%d, dim=%d\n",
568                             mp->getName().c_str(), mp->getPosition(), mp->getLocation(), dbc, nevents, m_dimension );
569                 debugOutput ( DEBUG_LEVEL_ULTRA_VERBOSE, "base=%p, target=%p, value=%08X\n",
570                             data, target_event, tmpval );
571
572             }
573             else
574             {
575                 // can't send a byte, either because there is no byte,
576                 // or because this would exceed the maximum rate
577                 tmpval=htonl (
578                         IEC61883_AM824_SET_LABEL ( 0,IEC61883_AM824_LABEL_MIDI_NO_DATA ) );
579             }
580
581             *target_event=tmpval;
582         }
583
584     }
585     return ok;
586 }
587
588 #if USE_SSE
589 typedef float v4sf __attribute__ ((vector_size (16)));
590 typedef int v4si __attribute__ ((vector_size (16)));
591 typedef int v2si __attribute__ ((vector_size (8)));
592
593 int AmdtpTransmitStreamProcessor::encodePortToMBLAEvents ( AmdtpAudioPort *p, quadlet_t *data,
594         unsigned int offset, unsigned int nevents )
595 {
596     static const float sse_multiplier[4] __attribute__((aligned(16))) = {
597         (float)(0x7FFFFF00),
598         (float)(0x7FFFFF00),
599         (float)(0x7FFFFF00),
600         (float)(0x7FFFFF00)
601     };
602
603     static const int sse_mask[4] __attribute__((aligned(16))) = {
604         0x40000000,  0x40000000,  0x40000000,  0x40000000
605     };
606
607     unsigned int out[4];
608
609     unsigned int j=0;
610     unsigned int read=0;
611
612     quadlet_t *target_event;
613
614     target_event= ( quadlet_t * ) ( data + p->getPosition() );
615
616     switch ( p->getDataType() )
617     {
618         default:
619         case Port::E_Int24:
620         {
621             quadlet_t *buffer= ( quadlet_t * ) ( p->getBufferAddress() );
622
623             assert ( nevents + offset <= p->getBufferSize() );
624
625             buffer+=offset;
626
627             for ( j = 0; j < nevents; j += 1 )   // decode max nsamples
628             {
629                 *target_event = htonl ( ( * ( buffer ) & 0x00FFFFFF ) | 0x40000000 );
630                 buffer++;
631                 target_event += m_dimension;
632             }
633         }
634         break;
635         case Port::E_Float:
636         {
637             const float multiplier = ( float ) ( 0x7FFFFF00 );
638             float *buffer= ( float * ) ( p->getBufferAddress() );
639
640             assert ( nevents + offset <= p->getBufferSize() );
641
642             buffer+=offset;
643
644             j=0;
645             if(read>3) {
646                 for (j = 0; j < read-3; j += 4) {
647                     asm("movups %[floatbuff], %%xmm0\n\t"
648                             "mulps %[ssemult], %%xmm0\n\t"
649                             "cvttps2pi %%xmm0, %[out1]\n\t"
650                             "movhlps %%xmm0, %%xmm0\n\t"
651                             "psrld $8, %[out1]\n\t"
652                             "cvttps2pi %%xmm0, %[out2]\n\t"
653                             "por %[mmxmask], %[out1]\n\t"
654                             "psrld $8, %[out2]\n\t"
655                             "por %[mmxmask], %[out2]\n\t"
656                         : [out1] "=&y" (*(v2si*)&out[0]),
657                     [out2] "=&y" (*(v2si*)&out[2])
658                         : [floatbuff] "m" (*(v4sf*)buffer),
659                     [ssemult] "x" (*(v4sf*)sse_multiplier),
660                     [mmxmask] "y" (*(v2si*)sse_mask)
661                         : "xmm0");
662                     buffer += 4;
663                     *target_event = htonl(out[0]);
664                     target_event += m_dimension;
665                     *target_event = htonl(out[1]);
666                     target_event += m_dimension;
667                     *target_event = htonl(out[2]);
668                     target_event += m_dimension;
669                     *target_event = htonl(out[3]);
670                     target_event += m_dimension;
671                 }
672             }
673             for(; j < read; ++j) {
674             // don't care for overflow
675                 float v = *buffer * multiplier;  // v: -231 .. 231
676                 unsigned int tmp = (int)v;
677                 *target_event = htonl((tmp >> 8) | 0x40000000);
678    
679                 buffer++;
680                 target_event += m_dimension;
681             }
682
683             asm volatile("emms");
684             break;
685         }
686         break;
687     }
688
689     return 0;
690 }
691
692 #else
693
694 int AmdtpTransmitStreamProcessor::encodePortToMBLAEvents ( AmdtpAudioPort *p, quadlet_t *data,
695         unsigned int offset, unsigned int nevents )
696 {
697     unsigned int j=0;
698
699     quadlet_t *target_event;
700
701     target_event= ( quadlet_t * ) ( data + p->getPosition() );
702
703     switch ( p->getDataType() )
704     {
705         default:
706         case Port::E_Int24:
707         {
708             quadlet_t *buffer= ( quadlet_t * ) ( p->getBufferAddress() );
709
710             assert ( nevents + offset <= p->getBufferSize() );
711
712             buffer+=offset;
713
714             for ( j = 0; j < nevents; j += 1 )   // decode max nsamples
715             {
716                 *target_event = htonl ( ( * ( buffer ) & 0x00FFFFFF ) | 0x40000000 );
717                 buffer++;
718                 target_event += m_dimension;
719             }
720         }
721         break;
722         case Port::E_Float:
723         {
724             const float multiplier = ( float ) ( 0x7FFFFF00 );
725             float *buffer= ( float * ) ( p->getBufferAddress() );
726
727             assert ( nevents + offset <= p->getBufferSize() );
728
729             buffer+=offset;
730
731             for ( j = 0; j < nevents; j += 1 )   // decode max nsamples
732             {
733
734                 // don't care for overflow
735                 float v = *buffer * multiplier;  // v: -231 .. 231
736                 unsigned int tmp = ( ( int ) v );
737                 *target_event = htonl ( ( tmp >> 8 ) | 0x40000000 );
738
739                 buffer++;
740                 target_event += m_dimension;
741             }
742         }
743         break;
744     }
745
746     return 0;
747 }
748 #endif
749
750 int AmdtpTransmitStreamProcessor::encodeSilencePortToMBLAEvents ( AmdtpAudioPort *p, quadlet_t *data,
751         unsigned int offset, unsigned int nevents )
752 {
753     unsigned int j=0;
754
755     quadlet_t *target_event;
756
757     target_event= ( quadlet_t * ) ( data + p->getPosition() );
758
759     switch ( p->getDataType() )
760     {
761         default:
762         case Port::E_Int24:
763         case Port::E_Float:
764         {
765             for ( j = 0; j < nevents; j += 1 )   // decode max nsamples
766             {
767                 *target_event = htonl ( 0x40000000 );
768                 target_event += m_dimension;
769             }
770         }
771         break;
772     }
773
774     return 0;
775 }
776
777 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.