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