root/trunk/libffado/src/libstreaming/motu/MotuReceiveStreamProcessor.cpp

Revision 1361, 21.1 kB (checked in by ppalmers, 14 years ago)

Merge 2.0 branch changes.

svn merge -r1349:HEAD svn+ssh://ffadosvn@ffado.org/ffado/branches/libffado-2.0

Line 
1 /*
2  * Copyright (C) 2005-2008 by Jonathan Woithe
3  * Copyright (C) 2005-2008 by Pieter Palmers
4  *
5  * This file is part of FFADO
6  * FFADO = Free Firewire (pro-)audio drivers for linux
7  *
8  * FFADO is based upon FreeBoB.
9  *
10  * This program is free software: you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation, either version 2 of the License, or
13  * (at your option) version 3 of the License.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
22  *
23  */
24
25
26 #include "libutil/float_cast.h"
27
28 #include "MotuReceiveStreamProcessor.h"
29 #include "MotuPort.h"
30 #include "../StreamProcessorManager.h"
31 #include "devicemanager.h"
32
33 #include "libieee1394/ieee1394service.h"
34 #include "libieee1394/IsoHandlerManager.h"
35 #include "libieee1394/cycletimer.h"
36
37 #include "libutil/ByteSwap.h"
38
39 #include <cstring>
40 #include <math.h>
41 #include <assert.h>
42
43 /* Provide more intuitive access to GCC's branch predition built-ins */
44 #define likely(x)   __builtin_expect((x),1)
45 #define unlikely(x) __builtin_expect((x),0)
46
47
48 namespace Streaming {
49
50 // A macro to extract specific bits from a native endian quadlet
51 #define get_bits(_d,_start,_len) (((_d)>>((_start)-(_len)+1)) & ((1<<(_len))-1))
52
53 // Convert an SPH timestamp as received from the MOTU to a full timestamp in ticks.
54 static inline uint32_t sphRecvToFullTicks(uint32_t sph, uint32_t ct_now) {
55
56 uint32_t timestamp = CYCLE_TIMER_TO_TICKS(sph & 0x1ffffff);
57 uint32_t now_cycles = CYCLE_TIMER_GET_CYCLES(ct_now);
58
59 uint32_t ts_sec = CYCLE_TIMER_GET_SECS(ct_now);
60     // If the cycles have wrapped, correct ts_sec so it represents when timestamp
61     // was received.  The timestamps sent by the MOTU are always 1 or two cycles
62     // in advance of the cycle timer (reasons unknown at this stage).  In addition,
63     // iso buffering can delay the arrival of packets for quite a number of cycles
64     // (have seen a delay >12 cycles).
65     // Every so often we also see sph wrapping ahead of ct_now, so deal with that
66     // too.
67     if (unlikely(CYCLE_TIMER_GET_CYCLES(sph) > now_cycles + 1000)) {
68         if (likely(ts_sec))
69             ts_sec--;
70         else
71             ts_sec = 127;
72     } else
73     if (unlikely(now_cycles > CYCLE_TIMER_GET_CYCLES(sph) + 1000)) {
74         if (unlikely(ts_sec == 127))
75             ts_sec = 0;
76         else
77             ts_sec++;
78     }
79     return timestamp + ts_sec*TICKS_PER_SECOND;
80 }
81
82 MotuReceiveStreamProcessor::MotuReceiveStreamProcessor(FFADODevice &parent, unsigned int event_size)
83     : StreamProcessor(parent, ePT_Receive)
84     , m_event_size( event_size )
85     , mb_head ( 0 )
86     , mb_tail ( 0 )
87 {
88     memset(&m_devctrls, 0, sizeof(m_devctrls));
89 }
90
91 unsigned int
92 MotuReceiveStreamProcessor::getMaxPacketSize() {
93     int framerate = m_Parent.getDeviceManager().getStreamProcessorManager().getNominalRate();
94     return framerate<=48000?616:(framerate<=96000?1032:1160);
95 }
96
97 unsigned int
98 MotuReceiveStreamProcessor::getAveragePacketSize()
99 {
100     // in one second we have 8000 packets
101     // containing FRAMERATE frames
102     // so on average bytes/packet: (8000 packet headers + FRAMERATE * frame_size) / 8000
103     #warning FIXME
104     int framerate = m_Parent.getDeviceManager().getStreamProcessorManager().getNominalRate();
105     return framerate<=48000?616:(framerate<=96000?1032:1160);
106 }
107
108 unsigned int
109 MotuReceiveStreamProcessor::getNominalFramesPerPacket() {
110     int framerate = m_Parent.getDeviceManager().getStreamProcessorManager().getNominalRate();
111     return framerate<=48000?8:(framerate<=96000?16:32);
112 }
113
114 bool
115 MotuReceiveStreamProcessor::prepareChild() {
116     debugOutput( DEBUG_LEVEL_VERBOSE, "Preparing (%p)...\n", this);
117
118     // prepare the framerate estimate
119     // FIXME: not needed anymore?
120     //m_ticks_per_frame = (TICKS_PER_SECOND*1.0) / ((float)m_Parent.getDeviceManager().getStreamProcessorManager().getNominalRate());
121
122     return true;
123 }
124
125
126 /**
127  * Processes packet header to extract timestamps and check if the packet is valid
128  * @param data
129  * @param length
130  * @param channel
131  * @param tag
132  * @param sy
133  * @param cycle
134  * @return
135  */
136 enum StreamProcessor::eChildReturnValue
137 MotuReceiveStreamProcessor::processPacketHeader(unsigned char *data, unsigned int length,
138                                                 unsigned char tag, unsigned char sy,
139                                                 uint32_t pkt_ctr)
140 {
141     if (length > 8) {
142         // The iso data blocks from the MOTUs comprise a CIP-like
143         // header followed by a number of events (8 for 1x rates, 16
144         // for 2x rates, 32 for 4x rates).
145         quadlet_t *quadlet = (quadlet_t *)data;
146         unsigned int dbs = get_bits(CondSwapFromBus32(quadlet[0]), 23, 8);  // Size of one event in terms of fdf_size
147         unsigned int fdf_size = get_bits(CondSwapFromBus32(quadlet[1]), 23, 8) == 0x22 ? 32:0; // Event unit size in bits
148
149         // Don't even attempt to process a packet if it isn't what we expect
150         // from a MOTU.  Yes, an FDF value of 32 bears little relationship
151         // to the actual data (24 bit integer) sent by the MOTU - it's one
152         // of those areas where MOTU have taken a curious detour around the
153         // standards.  Do this check early on because for invalid packets
154         // dbs may not be what we expect, potentially causing issues later
155         // on.
156         if (tag!=1 || fdf_size!=32 || dbs==0) {
157             return eCRV_Invalid;
158         }
159
160         // m_event_size is the event size in bytes
161         unsigned int n_events = (length-8) / m_event_size;
162
163         // Acquire the timestamp of the last frame in the packet just
164         // received.  Since every frame from the MOTU has its own timestamp
165         // we can just pick it straight from the packet.
166         uint32_t last_sph = CondSwapFromBus32(*(quadlet_t *)(data+8+(n_events-1)*m_event_size));
167         m_last_timestamp = sphRecvToFullTicks(last_sph, m_Parent.get1394Service().getCycleTimer());
168
169         return eCRV_OK;
170     } else {
171         return eCRV_Invalid;
172     }
173 }
174
175 /**
176  * extract the data from the packet
177  * @pre the IEC61883 packet is valid according to isValidPacket
178  * @param data
179  * @param length
180  * @param channel
181  * @param tag
182  * @param sy
183  * @param pkt_ctr
184  * @return
185  */
186 enum StreamProcessor::eChildReturnValue
187 MotuReceiveStreamProcessor::processPacketData(unsigned char *data, unsigned int length) {
188     // m_event_size should never be zero
189     unsigned int n_events = (length-8) / m_event_size;
190
191     // we have to keep in mind that there are also
192     // some packets buffered by the ISO layer,
193     // at most x=m_handler->getWakeupInterval()
194     // these contain at most x*syt_interval
195     // frames, meaning that we might receive
196     // this packet x*syt_interval*ticks_per_frame
197     // later than expected (the real receive time)
198     #ifdef DEBUG
199     if(isRunning()) {
200         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"STMP: %lluticks | tpf=%f\n",
201             m_last_timestamp, getTicksPerFrame());
202     }
203     #endif
204
205     if(m_data_buffer->writeFrames(n_events, (char *)(data+8), m_last_timestamp)) {
206         return eCRV_OK;
207     } else {
208         return eCRV_XRun;
209     }
210 }
211
212 /***********************************************
213  * Encoding/Decoding API                       *
214  ***********************************************/
215 /**
216  * \brief write received events to the port ringbuffers.
217  */
218 bool MotuReceiveStreamProcessor::processReadBlock(char *data,
219                        unsigned int nevents, unsigned int offset)
220 {
221     bool no_problem=true;
222
223     /* Scan incoming block for device control events */
224     decodeMotuCtrlEvents(data, nevents);
225
226     for ( PortVectorIterator it = m_Ports.begin();
227           it != m_Ports.end();
228           ++it ) {
229         if((*it)->isDisabled()) {continue;};
230
231         Port *port=(*it);
232
233         switch(port->getPortType()) {
234
235         case Port::E_Audio:
236             if(decodeMotuEventsToPort(static_cast<MotuAudioPort *>(*it), (quadlet_t *)data, offset, nevents)) {
237                 debugWarning("Could not decode packet data to port %s",(*it)->getName().c_str());
238                 no_problem=false;
239             }
240             break;
241         case Port::E_Midi:
242              if(decodeMotuMidiEventsToPort(static_cast<MotuMidiPort *>(*it), (quadlet_t *)data, offset, nevents)) {
243                  debugWarning("Could not decode packet midi data to port %s",(*it)->getName().c_str());
244                  no_problem=false;
245              }
246             break;
247
248         default: // ignore
249             break;
250         }
251     }
252     return no_problem;
253 }
254
255 signed int MotuReceiveStreamProcessor::decodeMotuEventsToPort(MotuAudioPort *p,
256         quadlet_t *data, unsigned int offset, unsigned int nevents)
257 {
258     unsigned int j=0;
259
260     // Use char here since a port's source address won't necessarily be
261     // aligned; use of an unaligned quadlet_t may cause issues on
262     // certain architectures.  Besides, the source (data coming directly
263     // from the MOTU) isn't structured in quadlets anyway; it mainly
264     // consists of packed 24-bit integers.
265
266     unsigned char *src_data;
267     src_data = (unsigned char *)data + p->getPosition();
268
269     switch(m_StreamProcessorManager.getAudioDataType()) {
270         default:
271         case StreamProcessorManager::eADT_Int24:
272             {
273                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
274
275                 assert(nevents + offset <= p->getBufferSize());
276
277                 // Offset is in frames, but each port is only a single
278                 // channel, so the number of frames is the same as the
279                 // number of quadlets to offset (assuming the port buffer
280                 // uses one quadlet per sample, which is the case currently).
281                 buffer+=offset;
282
283                 for(j = 0; j < nevents; j += 1) { // Decode nsamples
284                     *buffer = (*src_data<<16)+(*(src_data+1)<<8)+*(src_data+2);
285                     // Sign-extend highest bit of 24-bit int.
286                     // FIXME: this isn't strictly needed since E_Int24 is a 24-bit,
287                     // but doing so shouldn't break anything and makes the data
288                     // easier to deal with during debugging.
289                     if (*src_data & 0x80)
290                         *buffer |= 0xff000000;
291
292                     buffer++;
293                     src_data+=m_event_size;
294                 }
295             }
296             break;
297         case StreamProcessorManager::eADT_Float:
298             {
299                 const float multiplier = 1.0f / (float)(0x7FFFFF);
300                 float *buffer=(float *)(p->getBufferAddress());
301
302                 assert(nevents + offset <= p->getBufferSize());
303
304                 buffer+=offset;
305
306                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
307
308                     signed int v = (*src_data<<16)+(*(src_data+1)<<8)+*(src_data+2);
309                     /* Sign-extend highest bit of incoming 24-bit integer */
310                     if (*src_data & 0x80)
311                       v |= 0xff000000;
312                     *buffer = v * multiplier;
313                     buffer++;
314                     src_data+=m_event_size;
315                 }
316             }
317             break;
318     }
319
320     return 0;
321 }
322
323 int
324 MotuReceiveStreamProcessor::decodeMotuMidiEventsToPort(
325                       MotuMidiPort *p, quadlet_t *data,
326                       unsigned int offset, unsigned int nevents)
327 {
328     unsigned int j = 0;
329     unsigned char *src = NULL;
330
331     quadlet_t *buffer = (quadlet_t *)(p->getBufferAddress());
332     assert(nevents + offset <= p->getBufferSize());
333     buffer += offset;
334
335     // Zero the buffer
336     memset(buffer, 0, nevents*sizeof(*buffer));
337
338     // Get MIDI bytes if present in any frames within the packet.  MOTU MIDI
339     // data is sent as part of a 3-byte sequence starting at the port's
340     // position.  Some MOTUs (eg: the 828MkII) send more than one MIDI byte
341     // in some packets.  Since the FFADO MIDI layer requires a MIDI byte in
342     // only every 8th buffer position we allow for this by buffering the
343     // incoming data.  The buffer is small since it only has to cover for
344     // short-term excursions in the data rate.  Since the MIDI data
345     // originates on a physical MIDI bus the overall data rate is limited by
346     // the baud rate of that bus (31250), which is no more than one byte in
347     // 8 even for 1x sample rates.
348     src = (unsigned char *)data + p->getPosition();
349     // We assume that the buffer has been set up in such a way that the first
350     // element is correctly aligned for FFADOs MIDI layer.  The requirement
351     // is that actual MIDI bytes must be aligned to multiples of 8 samples. 
352
353     while (j < nevents) {
354         /* Most events don't have MIDI data bytes */
355         if (unlikely((*src & MOTU_KEY_MASK_MIDI) == MOTU_KEY_MASK_MIDI)) {
356             // A MIDI byte is in *(src+2).  Bit 24 is used to flag MIDI data
357             // as present once the data makes it to the output buffer.
358             midibuffer[mb_head++] = 0x01000000 | *(src+2);
359             mb_head &= RX_MIDIBUFFER_SIZE-1;
360             if (unlikely(mb_head == mb_tail)) {
361                 debugWarning("MOTU rx MIDI buffer overflow\n");
362                 /* Dump oldest byte.  This overflow can only happen if the
363                  * rate coming in from the hardware MIDI port grossly
364                  * exceeds the official MIDI baud rate of 31250 bps, so it
365                  * should never occur in practice.
366                  */
367                 mb_tail = (mb_tail + 1) & (RX_MIDIBUFFER_SIZE-1);
368             }
369         }
370         /* Write to the buffer if we're at an 8-sample boundary */
371         if (unlikely(!(j & 0x07))) {
372             if (mb_head != mb_tail) {
373                 *buffer = midibuffer[mb_tail++];
374                 mb_tail &= RX_MIDIBUFFER_SIZE-1;
375             }
376             buffer += 8;
377         }
378         j++;
379         src += m_event_size;
380     }
381
382     return 0;   
383 }
384
385 int
386 MotuReceiveStreamProcessor::decodeMotuCtrlEvents(
387                       char *data, unsigned int nevents)
388 {
389     unsigned int j = 0;
390     unsigned char *src = NULL;
391     unsigned char *arg = NULL;
392
393     // Get control bytes if present in any frames within the packet.  The
394     // device control messages start at (zero-based) byte 0x04 in the data
395     // stream.
396     src = (unsigned char *)data + 0x04;
397     arg = src+1;
398     while (j < nevents) {
399         unsigned int control_key = *src & ~MOTU_KEY_MASK_MIDI;
400        
401         if (m_devctrls.status == MOTU_DEVCTRL_INVALID) {
402             // Start syncing on reception of the sequence sync key which indicates
403             // mix bus 1 values are pending.  Acquisition will start when we see the
404             // first channel gain key after this.
405             if (control_key==MOTU_KEY_SEQ_SYNC && *arg==MOTU_KEY_SEQ_SYNC_MIXBUS1) {
406                  debugOutput(DEBUG_LEVEL_VERBOSE, "syncing device control status stream\n");
407                  m_devctrls.status = MOTU_DEVCTRL_SYNCING;
408             }
409         } else
410         if (m_devctrls.status == MOTU_DEVCTRL_SYNCING) {
411             // Start acquiring when we see a channel gain key for mixbus 1.
412             if (control_key == MOTU_KEY_SEQ_SYNC) {
413                 // Keep mixbus index updated since we don't execute the main parser until
414                 // we move to the initialising state.  Since we don't dereference this until
415                 // we know it's equal to 0 there's no need for bounds checking here.
416                 m_devctrls.mixbus_index = *arg;
417             } else
418             if (control_key==MOTU_KEY_CHANNEL_GAIN && m_devctrls.mixbus_index==0) {
419               debugOutput(DEBUG_LEVEL_VERBOSE, "initialising device control status\n");
420               m_devctrls.status = MOTU_DEVCTRL_INIT;
421             }
422         } else
423         if (m_devctrls.status == MOTU_DEVCTRL_INIT) {
424             // Consider ourselves fully initialised when a control sequence sync key
425             // arrives which takes things back to mixbus 1.
426             if (control_key==MOTU_KEY_SEQ_SYNC && *arg==MOTU_KEY_SEQ_SYNC_MIXBUS1 && m_devctrls.mixbus_index>0) {
427                 debugOutput(DEBUG_LEVEL_VERBOSE, "device control status valid: n_mixbuses=%d, n_channels=%d\n",
428                     m_devctrls.n_mixbuses, m_devctrls.n_channels);
429                 m_devctrls.status = MOTU_DEVCTRL_VALID;
430             }
431         }
432
433         if (m_devctrls.status==MOTU_DEVCTRL_INIT || m_devctrls.status==MOTU_DEVCTRL_VALID) {
434             unsigned int i;
435             switch (control_key) {
436                 case MOTU_KEY_SEQ_SYNC:
437                     if (m_devctrls.mixbus_index < MOTUFW_MAX_MIXBUSES) {
438                         if (m_devctrls.n_channels==0 && m_devctrls.mixbus[m_devctrls.mixbus_index].channel_gain_index!=0) {
439                             m_devctrls.n_channels = m_devctrls.mixbus[m_devctrls.mixbus_index].channel_gain_index;
440                         }
441                     }
442                     /* Mix bus to configure next is in bits 5-7 of the argument */
443                     m_devctrls.mixbus_index = (*arg >> 5);
444                     if (m_devctrls.mixbus_index >= MOTUFW_MAX_MIXBUSES) {
445                         debugWarning("MOTU cuemix value parser error: mix bus index %d exceeded maximum %d\n",
446                             m_devctrls.mixbus_index, MOTUFW_MAX_MIXBUSES);
447                     } else {
448                         if (m_devctrls.n_mixbuses < m_devctrls.mixbus_index+1) {
449                             m_devctrls.n_mixbuses = m_devctrls.mixbus_index+1;
450                         }
451                         m_devctrls.mixbus[m_devctrls.mixbus_index].channel_gain_index =
452                             m_devctrls.mixbus[m_devctrls.mixbus_index].channel_pan_index =
453                             m_devctrls.mixbus[m_devctrls.mixbus_index].channel_control_index = 0;
454                         }
455                     break;
456                 case MOTU_KEY_CHANNEL_GAIN:
457                     i = m_devctrls.mixbus[m_devctrls.mixbus_index].channel_gain_index++;
458                     if (m_devctrls.mixbus_index<MOTUFW_MAX_MIXBUSES && i<MOTUFW_MAX_MIXBUS_CHANNELS) {
459                         m_devctrls.mixbus[m_devctrls.mixbus_index].channel_gain[i] = *arg;
460                     }
461                     if (i >= MOTUFW_MAX_MIXBUS_CHANNELS) {
462                         debugWarning("MOTU cuemix value parser error: channel gain index %d exceeded maximum %d\n",
463                             i, MOTUFW_MAX_MIXBUS_CHANNELS);
464                     }
465                     break;
466                 case MOTU_KEY_CHANNEL_PAN:
467                     i = m_devctrls.mixbus[m_devctrls.mixbus_index].channel_pan_index++;
468                     if (m_devctrls.mixbus_index<MOTUFW_MAX_MIXBUSES && i<MOTUFW_MAX_MIXBUS_CHANNELS) {
469                         m_devctrls.mixbus[m_devctrls.mixbus_index].channel_pan[i] = *arg;
470                     }
471                     if (i >= MOTUFW_MAX_MIXBUS_CHANNELS) {
472                         debugWarning("MOTU cuemix value parser error: channel pan index %d exceeded maximum %d\n",
473                             i, MOTUFW_MAX_MIXBUS_CHANNELS);
474                     }
475                     break;
476                 case MOTU_KEY_CHANNEL_CTRL:
477                     i = m_devctrls.mixbus[m_devctrls.mixbus_index].channel_control_index++;
478                     if (m_devctrls.mixbus_index<MOTUFW_MAX_MIXBUSES && i<MOTUFW_MAX_MIXBUS_CHANNELS) {
479                         m_devctrls.mixbus[m_devctrls.mixbus_index].channel_control[i] = *arg;
480                     }
481                     if (i >= MOTUFW_MAX_MIXBUS_CHANNELS) {
482                         debugWarning("MOTU cuemix value parser error: channel control index %d exceeded maximum %d\n",
483                             i, MOTUFW_MAX_MIXBUS_CHANNELS);
484                     }
485                     break;
486                 case MOTU_KEY_MIXBUS_GAIN:
487                     if (m_devctrls.mixbus_index < MOTUFW_MAX_MIXBUSES) {
488                         m_devctrls.mixbus[m_devctrls.mixbus_index].bus_gain = *arg;
489                     }
490                     break;
491                 case MOTU_KEY_MIXBUS_DEST:
492                     if (m_devctrls.mixbus_index < MOTUFW_MAX_MIXBUSES) {
493                         m_devctrls.mixbus[m_devctrls.mixbus_index].bus_dest = *arg;
494                     }
495                     break;
496                 case MOTU_KEY_MAINOUT_VOL:
497                     m_devctrls.main_out_volume = *arg;
498                     break;
499                 case MOTU_KEY_PHONES_VOL:
500                     m_devctrls.phones_volume = *arg;
501                     break;
502                 case MOTU_KEY_PHONES_DEST:
503                     m_devctrls.phones_assign = *arg;
504                     break;
505                 case MOTU_KEY_INPUT_6dB_BOOST:
506                     m_devctrls.input_6dB_boost = *arg;
507                     break;
508                 case MOTU_KEY_INPUT_REF_LEVEL:
509                     m_devctrls.input_ref_level = *arg;
510                     break;
511                 case MOTU_KEY_MIDI:
512                     // MIDI is dealt with elsewhere, so just pass it over
513                     break;
514                 default:
515                     // Ignore any unknown keys or those we don't care about, at
516                     // least for now.
517                     break;
518             }
519         }
520         j++;
521         src += m_event_size;
522         arg += m_event_size;
523     }
524
525     return 0;   
526 }
527
528 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.