root/branches/ppalmers-streaming/src/libstreaming/motu/MotuReceiveStreamProcessor.cpp

Revision 733, 13.8 kB (checked in by ppalmers, 13 years ago)

adapt motu code to new SP base class (compiles, needs real testing)

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 library is free software; you can redistribute it and/or
10  * modify it under the terms of the GNU Lesser General Public
11  * License version 2.1, as published by the Free Software Foundation;
12  *
13  * This library is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
16  * Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with this library; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
21  * MA 02110-1301 USA
22  */
23
24 #include "MotuReceiveStreamProcessor.h"
25 #include "MotuPort.h"
26 #include "../StreamProcessorManager.h"
27
28 #include "../util/cycletimer.h"
29
30 #include <math.h>
31 #include <netinet/in.h>
32 #include <assert.h>
33
34 namespace Streaming {
35
36 // A macro to extract specific bits from a native endian quadlet
37 #define get_bits(_d,_start,_len) (((_d)>>((_start)-(_len)+1)) & ((1<<(_len))-1))
38
39 // Convert an SPH timestamp as received from the MOTU to a full timestamp in ticks.
40 static inline uint32_t sphRecvToFullTicks(uint32_t sph, uint32_t ct_now) {
41
42 uint32_t timestamp = CYCLE_TIMER_TO_TICKS(sph & 0x1ffffff);
43 uint32_t now_cycles = CYCLE_TIMER_GET_CYCLES(ct_now);
44
45 uint32_t ts_sec = CYCLE_TIMER_GET_SECS(ct_now);
46     // If the cycles have wrapped, correct ts_sec so it represents when timestamp
47     // was received.  The timestamps sent by the MOTU are always 1 or two cycles
48     // in advance of the cycle timer (reasons unknown at this stage).  In addition,
49     // iso buffering can delay the arrival of packets for quite a number of cycles
50     // (have seen a delay >12 cycles).
51     // Every so often we also see sph wrapping ahead of ct_now, so deal with that
52     // too.
53     if (CYCLE_TIMER_GET_CYCLES(sph) > now_cycles + 1000) {
54         if (ts_sec)
55             ts_sec--;
56         else
57             ts_sec = 127;
58     } else
59     if (now_cycles > CYCLE_TIMER_GET_CYCLES(sph) + 1000) {
60         if (ts_sec == 127)
61             ts_sec = 0;
62         else
63             ts_sec++;
64     }
65     return timestamp + ts_sec*TICKS_PER_SECOND;
66 }
67
68 MotuReceiveStreamProcessor::MotuReceiveStreamProcessor(int port, unsigned int event_size)
69     : StreamProcessor(ePT_Receive , port)
70     , m_event_size(event_size)
71 {}
72
73 unsigned int
74 MotuReceiveStreamProcessor::getMaxPacketSize() {
75     int framerate = m_manager->getNominalRate();
76     return framerate<=48000?616:(framerate<=96000?1032:1160);
77 }
78
79 unsigned int
80 MotuReceiveStreamProcessor::getNominalFramesPerPacket() {
81     int framerate = m_manager->getNominalRate();
82     return framerate<=48000?8:(framerate<=96000?16:32);
83 }
84
85 bool
86 MotuReceiveStreamProcessor::prepareChild() {
87     debugOutput( DEBUG_LEVEL_VERBOSE, "Preparing (%p)...\n", this);
88
89     // prepare the framerate estimate
90     // FIXME: not needed anymore?
91     //m_ticks_per_frame = (TICKS_PER_SECOND*1.0) / ((float)m_manager->getNominalRate());
92
93     debugOutput( DEBUG_LEVEL_VERBOSE, "Prepared for:\n");
94     debugOutput( DEBUG_LEVEL_VERBOSE, " Samplerate: %d\n",
95              m_manager->getNominalRate());
96     debugOutput( DEBUG_LEVEL_VERBOSE, " PeriodSize: %d, NbBuffers: %d\n",
97              m_manager->getPeriodSize(), m_manager->getNbBuffers());
98     debugOutput( DEBUG_LEVEL_VERBOSE, " Port: %d, Channel: %d\n",
99              m_port, m_channel);
100
101     return true;
102 }
103
104
105 /**
106  * Processes packet header to extract timestamps and check if the packet is valid
107  * @param data
108  * @param length
109  * @param channel
110  * @param tag
111  * @param sy
112  * @param cycle
113  * @param dropped
114  * @return
115  */
116 enum StreamProcessor::eChildReturnValue
117 MotuReceiveStreamProcessor::processPacketHeader(unsigned char *data, unsigned int length,
118                   unsigned char channel, unsigned char tag, unsigned char sy,
119                   unsigned int cycle, unsigned int dropped)
120 {
121     if (length > 8) {
122         // The iso data blocks from the MOTUs comprise a CIP-like
123         // header followed by a number of events (8 for 1x rates, 16
124         // for 2x rates, 32 for 4x rates).
125         quadlet_t *quadlet = (quadlet_t *)data;
126         unsigned int dbs = get_bits(ntohl(quadlet[0]), 23, 8);  // Size of one event in terms of fdf_size
127         unsigned int fdf_size = get_bits(ntohl(quadlet[1]), 23, 8) == 0x22 ? 32:0; // Event unit size in bits
128
129         // Don't even attempt to process a packet if it isn't what
130         // we expect from a MOTU.  Yes, an FDF value of 32 bears
131         // little relationship to the actual data (24 bit integer)
132         // sent by the MOTU - it's one of those areas where MOTU
133         // have taken a curious detour around the standards.
134         if (tag!=1 || fdf_size!=32) {
135             return eCRV_Invalid;
136         }
137
138         // put this after the check because event_length can become 0 on invalid packets
139         unsigned int event_length = (fdf_size * dbs) / 8;       // Event size in bytes
140         unsigned int n_events = (length-8) / event_length;
141
142         // Acquire the timestamp of the last frame in the packet just
143         // received.  Since every frame from the MOTU has its own timestamp
144         // we can just pick it straight from the packet.
145         uint32_t last_sph = ntohl(*(quadlet_t *)(data+8+(n_events-1)*event_length));
146         m_last_timestamp = sphRecvToFullTicks(last_sph, m_handler->getCycleTimer());
147         return eCRV_OK;
148     } else {
149         return eCRV_Invalid;
150     }
151 }
152
153 /**
154  * extract the data from the packet
155  * @pre the IEC61883 packet is valid according to isValidPacket
156  * @param data
157  * @param length
158  * @param channel
159  * @param tag
160  * @param sy
161  * @param cycle
162  * @param dropped
163  * @return
164  */
165 enum StreamProcessor::eChildReturnValue
166 MotuReceiveStreamProcessor::processPacketData(unsigned char *data, unsigned int length,
167                   unsigned char channel, unsigned char tag, unsigned char sy,
168                   unsigned int cycle, unsigned int dropped_cycles) {
169     quadlet_t* quadlet = (quadlet_t*) data;
170
171     unsigned int dbs = get_bits(ntohl(quadlet[0]), 23, 8);  // Size of one event in terms of fdf_size
172     unsigned int fdf_size = get_bits(ntohl(quadlet[1]), 23, 8) == 0x22 ? 32:0; // Event unit size in bits
173     // this is only called for packets that return eCRV_OK on processPacketHeader
174     // so event_length won't become 0
175     unsigned int event_length = (fdf_size * dbs) / 8;       // Event size in bytes
176     unsigned int n_events = (length-8) / event_length;
177
178     // we have to keep in mind that there are also
179     // some packets buffered by the ISO layer,
180     // at most x=m_handler->getWakeupInterval()
181     // these contain at most x*syt_interval
182     // frames, meaning that we might receive
183     // this packet x*syt_interval*ticks_per_frame
184     // later than expected (the real receive time)
185     #ifdef DEBUG
186     if(isRunning()) {
187         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"STMP: %lluticks | buff=%d, tpf=%f\n",
188             m_last_timestamp, m_handler->getWakeupInterval(), getTicksPerFrame());
189     }
190     #endif
191
192     if(m_data_buffer->writeFrames(n_events, (char *)(data+8), m_last_timestamp)) {
193         int dbc = get_bits(ntohl(quadlet[0]), 8, 8);
194         // process all ports that should be handled on a per-packet base
195         // this is MIDI for AMDTP (due to the need of DBC)
196         if(isRunning()) {
197             if (!decodePacketPorts((quadlet_t *)(data+8), n_events, dbc)) {
198                 debugWarning("Problem decoding Packet Ports\n");
199             }
200         }
201         return eCRV_OK;
202     } else {
203         return eCRV_XRun;
204     }
205 }
206
207 /***********************************************
208  * Encoding/Decoding API                       *
209  ***********************************************/
210 /**
211  * \brief write received events to the port ringbuffers.
212  */
213 bool MotuReceiveStreamProcessor::processReadBlock(char *data,
214                        unsigned int nevents, unsigned int offset)
215 {
216     bool no_problem=true;
217     for ( PortVectorIterator it = m_PeriodPorts.begin();
218           it != m_PeriodPorts.end();
219           ++it ) {
220         if((*it)->isDisabled()) {continue;};
221
222         //FIXME: make this into a static_cast when not DEBUG?
223         Port *port=dynamic_cast<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         // midi is a packet based port, don't process
234         //    case MotuPortInfo::E_Midi:
235         //        break;
236
237         default: // ignore
238             break;
239         }
240     }
241     return no_problem;
242 }
243
244 /**
245  * @brief decode a packet for the packet-based ports
246  *
247  * @param data Packet data
248  * @param nevents number of events in data (including events of other ports & port types)
249  * @param dbc DataBlockCount value for this packet
250  * @return true if all successfull
251  */
252 bool MotuReceiveStreamProcessor::decodePacketPorts(quadlet_t *data, unsigned int nevents,
253         unsigned int dbc) {
254     bool ok=true;
255
256     // Use char here since the source address won't necessarily be
257     // aligned; use of an unaligned quadlet_t may cause issues on
258     // certain architectures.  Besides, the source for MIDI data going
259     // directly to the MOTU isn't structured in quadlets anyway; it is a
260     // sequence of 3 unaligned bytes.
261     unsigned char *src = NULL;
262
263     for ( PortVectorIterator it = m_PacketPorts.begin();
264         it != m_PacketPorts.end();
265         ++it ) {
266
267         Port *port=dynamic_cast<Port *>(*it);
268         assert(port); // this should not fail!!
269
270         // Currently the only packet type of events for MOTU
271         // is MIDI in mbla.  However in future control data
272         // might also be sent via "packet" events, so allow
273         // for this possible expansion.
274
275         // FIXME: MIDI input is completely untested at present.
276         switch (port->getPortType()) {
277             case Port::E_Midi: {
278                 MotuMidiPort *mp=static_cast<MotuMidiPort *>(*it);
279                 signed int sample;
280                 unsigned int j = 0;
281                 // Get MIDI bytes if present anywhere in the
282                 // packet.  MOTU MIDI data is sent using a
283                 // 3-byte sequence starting at the port's
284                 // position.  It's thought that there can never
285                 // be more than one MIDI byte per packet, but
286                 // for completeness we'll check the entire packet
287                 // anyway.
288                 src = (unsigned char *)data + mp->getPosition();
289                 while (j < nevents) {
290                     if (*src==0x01 && *(src+1)==0x00) {
291                         sample = *(src+2);
292                         if (!mp->writeEvent(&sample)) {
293                             debugWarning("MIDI packet port events lost\n");
294                             ok = false;
295                         }
296                     }
297                     j++;
298                     src += m_event_size;
299                 }
300                 break;
301             }
302             default:
303                 debugOutput(DEBUG_LEVEL_VERBOSE, "Unknown packet-type port format %d\n",port->getPortType());
304                 return ok;
305               }
306     }
307
308     return ok;
309 }
310
311 signed int MotuReceiveStreamProcessor::decodeMotuEventsToPort(MotuAudioPort *p,
312         quadlet_t *data, unsigned int offset, unsigned int nevents)
313 {
314     unsigned int j=0;
315
316     // Use char here since a port's source address won't necessarily be
317     // aligned; use of an unaligned quadlet_t may cause issues on
318     // certain architectures.  Besides, the source (data coming directly
319     // from the MOTU) isn't structured in quadlets anyway; it mainly
320     // consists of packed 24-bit integers.
321
322     unsigned char *src_data;
323     src_data = (unsigned char *)data + p->getPosition();
324
325     switch(p->getDataType()) {
326         default:
327         case Port::E_Int24:
328             {
329                 quadlet_t *buffer=(quadlet_t *)(p->getBufferAddress());
330
331                 assert(nevents + offset <= p->getBufferSize());
332
333                 // Offset is in frames, but each port is only a single
334                 // channel, so the number of frames is the same as the
335                 // number of quadlets to offset (assuming the port buffer
336                 // uses one quadlet per sample, which is the case currently).
337                 buffer+=offset;
338
339                 for(j = 0; j < nevents; j += 1) { // Decode nsamples
340                     *buffer = (*src_data<<16)+(*(src_data+1)<<8)+*(src_data+2);
341                     // Sign-extend highest bit of 24-bit int.
342                     // FIXME: this isn't strictly needed since E_Int24 is a 24-bit,
343                     // but doing so shouldn't break anything and makes the data
344                     // easier to deal with during debugging.
345                     if (*src_data & 0x80)
346                         *buffer |= 0xff000000;
347
348                     buffer++;
349                     src_data+=m_event_size;
350                 }
351             }
352             break;
353         case Port::E_Float:
354             {
355                 const float multiplier = 1.0f / (float)(0x7FFFFF);
356                 float *buffer=(float *)(p->getBufferAddress());
357
358                 assert(nevents + offset <= p->getBufferSize());
359
360                 buffer+=offset;
361
362                 for(j = 0; j < nevents; j += 1) { // decode max nsamples
363
364                     unsigned int v = (*src_data<<16)+(*(src_data+1)<<8)+*(src_data+2);
365
366                     // sign-extend highest bit of 24-bit int
367                     int tmp = (int)(v << 8) / 256;
368
369                     *buffer = tmp * multiplier;
370
371                     buffer++;
372                     src_data+=m_event_size;
373                 }
374             }
375             break;
376     }
377
378     return 0;
379 }
380
381 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.