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

Revision 1136, 21.1 kB (checked in by ppalmers, 16 years ago)

keep bus direction in endian swapping functions

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