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

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