root/trunk/libffado/src/libstreaming/AmdtpSlaveStreamProcessor.cpp

Revision 506, 17.9 kB (checked in by ppalmers, 17 years ago)

compile fix for the slave stream sp

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 "AmdtpSlaveStreamProcessor.h"
25
26 #include "cycletimer.h"
27
28 #include <netinet/in.h>
29 #include <assert.h>
30
31 // in ticks
32 #define TRANSMIT_TRANSFER_DELAY 9000U
33 // the number of cycles to send a packet in advance of it's timestamp
34 #define TRANSMIT_ADVANCE_CYCLES 1U
35
36 namespace Streaming {
37
38 /* transmit */
39 AmdtpSlaveTransmitStreamProcessor::AmdtpSlaveTransmitStreamProcessor(int port, int framerate, int dimension)
40         : AmdtpTransmitStreamProcessor(port, framerate, dimension)
41 {
42
43 }
44
45 AmdtpSlaveTransmitStreamProcessor::~AmdtpSlaveTransmitStreamProcessor() {
46
47 }
48
49 enum raw1394_iso_disposition
50 AmdtpSlaveTransmitStreamProcessor::getPacket(unsigned char *data, unsigned int *length,
51                   unsigned char *tag, unsigned char *sy,
52                   int cycle, unsigned int dropped, unsigned int max_length) {
53
54     struct iec61883_packet *packet = (struct iec61883_packet *) data;
55     if (cycle<0) return RAW1394_ISO_OK;
56
57     m_last_cycle=cycle;
58
59     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"Xmit handler for cycle %d, (running=%d, enabled=%d,%d)\n",
60         cycle, m_running, m_disabled, m_is_disabled);
61
62 #ifdef DEBUG
63     if(dropped>0) {
64         debugWarning("Dropped %d packets on cycle %d\n",dropped, cycle);
65     }
66 #endif
67
68     // calculate & preset common values
69
70     /* Our node ID can change after a bus reset, so it is best to fetch
71      * our node ID for each packet. */
72     packet->sid = getNodeId() & 0x3f;
73
74     packet->dbs = m_dimension;
75     packet->fn = 0;
76     packet->qpc = 0;
77     packet->sph = 0;
78     packet->reserved = 0;
79     packet->dbc = m_dbc;
80     packet->eoh1 = 2;
81     packet->fmt = IEC61883_FMT_AMDTP;
82
83     *tag = IEC61883_TAG_WITH_CIP;
84     *sy = 0;
85
86     // determine if we want to send a packet or not
87     // note that we can't use getCycleTimer directly here,
88     // because packets are queued in advance. This means that
89     // we the packet we are constructing will be sent out
90     // on 'cycle', not 'now'.
91     unsigned int ctr=m_handler->getCycleTimer();
92     int now_cycles = (int)CYCLE_TIMER_GET_CYCLES(ctr);
93
94     // the difference between the cycle this
95     // packet is intended for and 'now'
96     int cycle_diff = diffCycles(cycle, now_cycles);
97
98 #ifdef DEBUG
99     if(m_running && (cycle_diff < 0)) {
100         debugWarning("Requesting packet for cycle %04d which is in the past (now=%04dcy)\n",
101             cycle, now_cycles);
102     }
103 #endif
104
105     // as long as the cycle parameter is not in sync with
106     // the current time, the stream is considered not
107     // to be 'running'
108     // NOTE: this works only at startup
109     if (!m_running && cycle_diff >= 0 && cycle >= 0) {
110         debugOutput(DEBUG_LEVEL_VERBOSE, "Xmit StreamProcessor %p started running at cycle %d\n",this, cycle);
111         m_running=true;
112
113         uint32_t ticks_to_advance = TICKS_PER_CYCLE * TRANSMIT_ADVANCE_CYCLES;
114
115         // if cycle lies cycle_diff cycles in the future, the
116         // timestamp for it corresponds to
117         // now+cycle_diff * TICKS_PER_CYCLE
118         ticks_to_advance += cycle_diff * TICKS_PER_CYCLE;
119
120         // determine the 'now' time in ticks
121         uint32_t cycle_timer=CYCLE_TIMER_TO_TICKS(ctr);
122
123         cycle_timer = addTicks(cycle_timer, ticks_to_advance);
124         m_data_buffer->setBufferHeadTimestamp(cycle_timer);
125     }
126
127     uint64_t ts_head;
128     int fc;
129     if (!m_disabled && m_is_disabled) { // this means that we are trying to enable
130         // check if we are on or past the enable point
131         int cycles_past_enable=diffCycles(cycle, m_cycle_to_enable_at);
132
133         if (cycles_past_enable >= 0) {
134             m_is_disabled=false;
135
136             debugOutput(DEBUG_LEVEL_VERBOSE,"Enabling StreamProcessor %p at %u\n", this, cycle);
137
138             // initialize the buffer head & tail
139             ffado_timestamp_t ts_head_tmp;
140             m_SyncSource->m_data_buffer->getBufferHeadTimestamp(&ts_head_tmp, &fc); // thread safe
141             ts_head=(uint64_t)ts_head_tmp;
142
143             // the number of cycles the sync source lags (> 0)
144             // or leads (< 0)
145             int sync_lag_cycles=diffCycles(cycle, m_SyncSource->getLastCycle());
146
147             // account for the cycle lag between sync SP and this SP
148             // the last update of the sync source's timestamps was sync_lag_cycles
149             // cycles before the cycle we are calculating the timestamp for.
150             // if we were to use one-frame buffers, you would expect the
151             // frame that is sent on cycle CT to have a timestamp T1.
152             // ts_head however is for cycle CT-sync_lag_cycles, and lies
153             // therefore sync_lag_cycles * TICKS_PER_CYCLE earlier than
154             // T1.
155             ts_head = addTicks(ts_head, (sync_lag_cycles) * TICKS_PER_CYCLE);
156
157             ts_head = substractTicks(ts_head, TICKS_PER_CYCLE);
158
159             // account for the number of cycles we are too late to enable
160             ts_head = addTicks(ts_head, cycles_past_enable * TICKS_PER_CYCLE);
161
162             // account for one extra packet of frames
163             ts_head = substractTicks(ts_head,
164                         (uint32_t)((float)m_syt_interval * m_SyncSource->m_data_buffer->getRate()));
165
166             m_data_buffer->setBufferHeadTimestamp(ts_head);
167
168             #ifdef DEBUG
169             if ((unsigned int)m_data_buffer->getFrameCounter() != m_data_buffer->getBufferSize()) {
170                 debugWarning("m_data_buffer->getFrameCounter() != m_data_buffer->getBufferSize()\n");
171             }
172             #endif
173             debugOutput(DEBUG_LEVEL_VERBOSE,"XMIT TS SET: TS=%10llu, LAG=%03d, FC=%4d\n",
174                             ts_head, sync_lag_cycles, m_data_buffer->getFrameCounter());
175         } else {
176             debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
177                         "will enable StreamProcessor %p at %u, now is %d\n",
178                         this, m_cycle_to_enable_at, cycle);
179         }
180     } else if (m_disabled && !m_is_disabled) {
181         // trying to disable
182         debugOutput(DEBUG_LEVEL_VERBOSE,"disabling StreamProcessor %p at %u\n",
183                     this, cycle);
184         m_is_disabled=true;
185     }
186
187     // the base timestamp is the one of the next sample in the buffer
188     ffado_timestamp_t ts_head_tmp;
189     m_data_buffer->getBufferHeadTimestamp(&ts_head_tmp, &fc); // thread safe
190     ts_head=(uint64_t)ts_head_tmp;
191
192     // we send a packet some cycles in advance, to avoid the
193     // following situation:
194     // suppose we are only a few ticks away from
195     // the moment to send this packet. therefore we decide
196     // not to send the packet, but send it in the next cycle.
197     // This means that the next time point will be 3072 ticks
198     // later, making that the timestamp will be expired when the
199     // packet is sent, unless TRANSFER_DELAY > 3072.
200     // this means that we need at least one cycle of extra buffering.
201     uint32_t ticks_to_advance = TICKS_PER_CYCLE * TRANSMIT_ADVANCE_CYCLES;
202
203     // if cycle lies cycle_diff cycles in the future, we should
204     // queue this packet cycle_diff * TICKS_PER_CYCLE earlier than
205     // we would if it were to be sent immediately.
206     ticks_to_advance += cycle_diff * TICKS_PER_CYCLE;
207
208     // determine the 'now' time in ticks
209     uint32_t cycle_timer=CYCLE_TIMER_TO_TICKS(ctr);
210
211     cycle_timer = addTicks(cycle_timer, ticks_to_advance);
212
213     // time until the packet is to be sent (if > 0: send packet)
214     int32_t until_next=diffTicks(ts_head, cycle_timer);
215
216     // if until_next < 0 we should send a filled packet
217     // otherwise we should send a NO-DATA packet
218     if((until_next<0) && (m_running)) {
219         // add the transmit transfer delay to construct the playout time (=SYT timestamp)
220         uint32_t ts_packet=addTicks(ts_head, TRANSMIT_TRANSFER_DELAY);
221
222         // if we are disabled, send a silent packet
223         // and advance the buffer head timestamp
224         if(m_is_disabled) {
225
226             transmitSilenceBlock((char *)(data+8), m_syt_interval, 0);
227             m_dbc += fillDataPacketHeader(packet, length, ts_packet);
228
229             debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "XMIT SYNC: CY=%04u TSH=%011llu TSP=%011lu\n",
230                 cycle, ts_head, ts_packet);
231
232             // update the base timestamp
233             uint32_t ts_step=(uint32_t)((float)(m_syt_interval)
234                              *m_SyncSource->m_data_buffer->getRate());
235
236             // the next buffer head timestamp
237             ts_head=addTicks(ts_head,ts_step);
238             m_data_buffer->setBufferHeadTimestamp(ts_head);
239
240             // defer to make sure we get to be enabled asap
241             return RAW1394_ISO_DEFER;
242
243         } else { // enabled & packet due, read from the buffer
244             if (m_data_buffer->readFrames(m_syt_interval, (char *)(data + 8))) {
245                 m_dbc += fillDataPacketHeader(packet, length, ts_packet);
246
247                 // process all ports that should be handled on a per-packet base
248                 // this is MIDI for AMDTP (due to the need of DBC)
249                 if (!encodePacketPorts((quadlet_t *)(data+8), m_syt_interval, packet->dbc)) {
250                     debugWarning("Problem encoding Packet Ports\n");
251                 }
252
253                 debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "XMIT DATA: CY=%04u TSH=%011llu TSP=%011lu\n",
254                     cycle, ts_head, ts_packet);
255
256                 return RAW1394_ISO_OK;
257
258             } else if (now_cycles<cycle) {
259                 // we can still postpone the queueing of the packets
260                 // because the ISO transmit packet buffer is not empty yet
261                 return RAW1394_ISO_AGAIN;
262
263             } else { // there is no more data in the ringbuffer
264                 // compose a silent packet, we should always
265                 // send a valid packet
266                 transmitSilenceBlock((char *)(data+8), m_syt_interval, 0);
267                 m_dbc += fillDataPacketHeader(packet, length, ts_packet);
268
269                 debugWarning("Transmit buffer underrun (now %d, queue %d, target %d)\n",
270                         now_cycles, cycle, TICKS_TO_CYCLES(ts_packet));
271                 // signal underrun
272                 m_xruns++;
273                 // disable the processing, will be re-enabled when
274                 // the xrun is handled
275                 m_disabled=true;
276                 m_is_disabled=true;
277
278                 return RAW1394_ISO_DEFER;
279             }
280         }
281
282     } else { // no packet due, send no-data packet
283         debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "XMIT NONE: CY=%04u TSH=%011llu\n",
284                 cycle, ts_head);
285
286         m_dbc += fillNoDataPacketHeader(packet, length);
287         return RAW1394_ISO_DEFER;
288     }
289
290     // we shouldn't get here
291     return RAW1394_ISO_ERROR;
292
293 }
294
295 /* --------------------- RECEIVE ----------------------- */
296
297 AmdtpSlaveReceiveStreamProcessor::AmdtpSlaveReceiveStreamProcessor(int port, int framerate, int dimension)
298     : AmdtpReceiveStreamProcessor(port, framerate, dimension)
299 {}
300
301 AmdtpSlaveReceiveStreamProcessor::~AmdtpSlaveReceiveStreamProcessor()
302 {}
303
304 enum raw1394_iso_disposition
305 AmdtpSlaveReceiveStreamProcessor::putPacket(unsigned char *data, unsigned int length,
306                   unsigned char channel, unsigned char tag, unsigned char sy,
307                   unsigned int cycle, unsigned int dropped) {
308
309     enum raw1394_iso_disposition retval=RAW1394_ISO_OK;
310     m_last_cycle=cycle;
311
312     struct iec61883_packet *packet = (struct iec61883_packet *) data;
313     assert(packet);
314
315 #ifdef DEBUG
316     if(dropped>0) {
317         debugWarning("Dropped %d packets on cycle %d\n",dropped, cycle);
318     }
319
320     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"ch%2u: CY=%4u, SYT=%08X (%4ucy + %04uticks) (running=%d, disabled=%d,%d)\n",
321         channel, cycle,ntohs(packet->syt),
322         CYCLE_TIMER_GET_CYCLES(ntohs(packet->syt)), CYCLE_TIMER_GET_OFFSET(ntohs(packet->syt)),
323         m_running,m_disabled,m_is_disabled);
324
325     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
326         "RCV: CH = %d, FDF = %X. SYT = %6d, DBS = %3d, DBC = %3d, FMT = %3d, LEN = %4d\n",
327         channel, packet->fdf,
328         packet->syt,
329         packet->dbs,
330         packet->dbc,
331         packet->fmt,
332         length);
333
334 #endif
335     // check our enable status
336     if (!m_disabled && m_is_disabled) { // this means that we are trying to enable
337         // check if we are on or past the enable point
338         int cycles_past_enable=diffCycles(cycle, m_cycle_to_enable_at);
339
340         if (cycles_past_enable >= 0) {
341             m_is_disabled=false;
342             debugOutput(DEBUG_LEVEL_VERBOSE,"Enabling StreamProcessor %p at %d (SYT=%04X)\n",
343                 this, cycle, ntohs(packet->syt));
344             // the previous timestamp is the one we need to start with
345             // because we're going to update the buffer again this loop
346             // using writeframes
347             m_data_buffer->setBufferTailTimestamp(m_last_timestamp);
348
349         } else {
350             debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
351                 "will enable StreamProcessor %p at %u, now is %d\n",
352                     this, m_cycle_to_enable_at, cycle);
353         }
354     } else if (m_disabled && !m_is_disabled) {
355         // trying to disable
356         debugOutput(DEBUG_LEVEL_VERBOSE,"disabling StreamProcessor %p at %u\n", this, cycle);
357         m_is_disabled=true;
358     }
359
360     // check if this is a valid packet
361     if((packet->syt != 0xFFFF)
362        && (packet->fdf != 0xFF)
363        && (packet->fmt == 0x10)
364        && (packet->dbs>0)
365        && (length>=2*sizeof(quadlet_t))) {
366
367         unsigned int nevents=((length / sizeof (quadlet_t)) - 2)/packet->dbs;
368
369         //=> store the previous timestamp
370         m_last_timestamp2=m_last_timestamp;
371
372         //=> convert the SYT to a full timestamp in ticks
373         m_last_timestamp=sytRecvToFullTicks((uint32_t)ntohs(packet->syt),
374                                         cycle, m_handler->getCycleTimer());
375
376         debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "RECV: CY=%04u TS=%011llu\n",
377                 cycle, m_last_timestamp);
378
379         // we have to keep in mind that there are also
380         // some packets buffered by the ISO layer,
381         // at most x=m_handler->getWakeupInterval()
382         // these contain at most x*syt_interval
383         // frames, meaning that we might receive
384         // this packet x*syt_interval*ticks_per_frame
385         // later than expected (the real receive time)
386         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"STMP: %lluticks | buff=%d, syt_interval=%d, tpf=%f\n",
387             m_last_timestamp, m_handler->getWakeupInterval(),m_syt_interval,getTicksPerFrame());
388
389         //=> signal that we're running (if we are)
390         if(!m_running && nevents && m_last_timestamp2 && m_last_timestamp) {
391             debugOutput(DEBUG_LEVEL_VERBOSE,"Receive StreamProcessor %p started running at %d\n", this, cycle);
392             m_running=true;
393         }
394
395         //=> don't process the stream samples when it is not enabled.
396         if(m_is_disabled) {
397
398             // we keep track of the timestamp here
399             // this makes sure that we will have a somewhat accurate
400             // estimate as to when a period might be ready. i.e. it will not
401             // be ready earlier than this timestamp + period time
402
403             // the next (possible) sample is not this one, but lies
404             // SYT_INTERVAL * rate later
405             uint64_t ts=addTicks(m_last_timestamp,
406                                  (uint64_t)((float)m_syt_interval * getTicksPerFrame()));
407
408             // set the timestamp as if there will be a sample put into
409             // the buffer by the next packet.
410             m_data_buffer->setBufferTailTimestamp(ts);
411
412             return RAW1394_ISO_DEFER;
413         }
414
415         #ifdef DEBUG_OFF
416         if((cycle % 1000) == 0)
417         {
418             uint32_t syt = (uint32_t)ntohs(packet->syt);
419             uint32_t now=m_handler->getCycleTimer();
420             uint32_t now_ticks=CYCLE_TIMER_TO_TICKS(now);
421
422             uint32_t test_ts=sytRecvToFullTicks(syt, cycle, now);
423
424             debugOutput(DEBUG_LEVEL_VERBOSE, "R %04d: SYT=%08X,            CY=%02d OFF=%04d\n",
425                 cycle, syt, CYCLE_TIMER_GET_CYCLES(syt), CYCLE_TIMER_GET_OFFSET(syt)
426                 );
427             debugOutput(DEBUG_LEVEL_VERBOSE, "R %04d: NOW=%011lu, SEC=%03u CY=%02u OFF=%04u\n",
428                 cycle, now_ticks, CYCLE_TIMER_GET_SECS(now), CYCLE_TIMER_GET_CYCLES(now), CYCLE_TIMER_GET_OFFSET(now)
429                 );
430             debugOutput(DEBUG_LEVEL_VERBOSE, "R %04d: TSS=%011lu, SEC=%03u CY=%02u OFF=%04u\n",
431                 cycle, test_ts, TICKS_TO_SECS(test_ts), TICKS_TO_CYCLES(test_ts), TICKS_TO_OFFSET(test_ts)
432                 );
433         }
434         #endif
435
436         //=> process the packet
437         // add the data payload to the ringbuffer
438         if(m_data_buffer->writeFrames(nevents, (char *)(data+8), m_last_timestamp)) {
439             retval=RAW1394_ISO_OK;
440
441             // process all ports that should be handled on a per-packet base
442             // this is MIDI for AMDTP (due to the need of DBC)
443             if (!decodePacketPorts((quadlet_t *)(data+8), nevents, packet->dbc)) {
444                 debugWarning("Problem decoding Packet Ports\n");
445                 retval=RAW1394_ISO_DEFER;
446             }
447
448         } else {
449
450             debugWarning("Receive buffer overrun (cycle %d, FC=%d, PC=%d)\n",
451                  cycle, m_data_buffer->getFrameCounter(), m_handler->getPacketCount());
452
453             m_xruns++;
454
455             // disable the processing, will be re-enabled when
456             // the xrun is handled
457             m_disabled=true;
458             m_is_disabled=true;
459
460             retval=RAW1394_ISO_DEFER;
461         }
462     }
463
464     return retval;
465 }
466
467 } // end of namespace Streaming
Note: See TracBrowser for help on using the browser.