1/*
2Copyright (c) 2012, Broadcom Europe Ltd
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without
6modification, are permitted provided that the following conditions are met:
7 * Redistributions of source code must retain the above copyright
8 notice, this list of conditions and the following disclaimer.
9 * Redistributions in binary form must reproduce the above copyright
10 notice, this list of conditions and the following disclaimer in the
11 documentation and/or other materials provided with the distribution.
12 * Neither the name of the copyright holder nor the
13 names of its contributors may be used to endorse or promote products
14 derived from this software without specific prior written permission.
15
16THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26*/
27
28#include "mmal.h"
29#include "core/mmal_component_private.h"
30#include "core/mmal_port_private.h"
31#include "util/mmal_util_rational.h"
32#include "util/mmal_list.h"
33#include "mmal_logging.h"
34
35
36#define CLOCK_PORTS_NUM 5
37
38#define MAX_CLOCK_EVENT_SLOTS 16
39
40#define DEFAULT_FRAME_RATE 30 /* frames per second */
41#define DEFAULT_CLOCK_LATENCY 60000 /* microseconds */
42
43#define FILTER_DURATION 2 /* seconds */
44#define MAX_FILTER_LENGTH 180 /* samples */
45
46#define MAX_TIME (~(1LL << 63)) /* microseconds */
47#define MIN_TIME (1LL << 63) /* microseconds */
48
49#define ABS(a) ((a) < 0 ? -(a) : (a))
50#define MIN(a,b) ((a) < (b) ? (a) : (b))
51#define MAX(a,b) ((a) > (b) ? (a) : (b))
52
53/** Set to 1 to enable additional stream timing log
54 * messages used for debugging the clock algorithm */
55#define ENABLE_ADDITIONAL_LOGGING 0
56static int clock_additional_logging = ENABLE_ADDITIONAL_LOGGING;
57
58/*****************************************************************************/
59typedef int64_t TIME_T;
60
61typedef struct FILTER_T
62{
63 uint32_t first; /**< index to the oldest sample */
64 uint32_t last; /**< index to the most recent sample*/
65 uint32_t count; /**< total number of samples in the filter */
66 uint32_t length; /**< maximum number of samples */
67 TIME_T sum; /**< sum of all samples currently in the filter */
68 TIME_T h[MAX_FILTER_LENGTH]; /**< filter history */
69} FILTER_T;
70
71/** Frame statistics for a stream */
72typedef struct CLOCK_STREAM_T
73{
74 uint32_t id; /**< for debug purposes */
75
76 MMAL_BOOL_T started; /**< TRUE at least one frame has been received */
77
78 TIME_T pts; /**< most recent time-stamp seen */
79 TIME_T stc; /**< most recent wall-time seen */
80
81 TIME_T mt_off; /**< offset of the current time stamp from the
82 arrival time, i.e. PTS - STC */
83 TIME_T mt_off_avg; /**< rolling average of the media time offset */
84 TIME_T mt_off_std; /**< approximate standard deviation of the media
85 time offset */
86
87 FILTER_T avg_filter; /**< moving average filter */
88 FILTER_T std_filter; /**< (approximate) standard deviation filter */
89} CLOCK_STREAM_T;
90
91/** Clock stream events */
92typedef enum CLOCK_STREAM_EVENT_T
93{
94 CLOCK_STREAM_EVENT_NONE,
95 CLOCK_STREAM_EVENT_STARTED, /**< first data received */
96 CLOCK_STREAM_EVENT_DISCONT, /**< discontinuity detected */
97 CLOCK_STREAM_EVENT_FRAME_COMPLETE, /**< complete frame received */
98} CLOCK_STREAM_EVENT_T;
99
100/** Clock port event */
101typedef struct CLOCK_PORT_EVENT_T
102{
103 MMAL_LIST_ELEMENT_T link; /**< must be first */
104 MMAL_PORT_T *port; /**< clock port where the event occurred */
105 MMAL_CLOCK_EVENT_T event; /**< event data */
106} CLOCK_PORT_EVENT_T;
107
108/** Clock component context */
109typedef struct MMAL_COMPONENT_MODULE_T
110{
111 MMAL_STATUS_T status; /**< current status of the component */
112
113 MMAL_BOOL_T clock_discont; /**< TRUE -> clock discontinuity detected */
114
115 uint32_t stream_min_id; /**< id of selected minimum stream (debugging only) */
116 uint32_t stream_max_id; /**< if of selected maximum stream (debugging only) */
117
118 TIME_T mt_off_target; /**< target clock media time offset */
119 TIME_T mt_off_clk; /**< current clock media time offset */
120
121 TIME_T adj_p; /**< proportional clock adjustment */
122 TIME_T adj_m; /**< clock adjustment factor (between 1 and 0) */
123 TIME_T adj; /**< final clock adjustment */
124
125 TIME_T stc_at_update; /**< the value of the STC the last time the clocks
126 were updated */
127
128 TIME_T frame_duration; /**< one frame period (microseconds) */
129 MMAL_RATIONAL_T frame_rate; /**< frame rate set by the client */
130 uint32_t frame_rate_log2; /**< frame rate expressed as a power of two */
131
132 MMAL_RATIONAL_T scale; /**< current clock scale factor */
133 MMAL_BOOL_T pending_scale; /**< TRUE -> scale change is pending */
134
135 MMAL_CLOCK_LATENCY_T latency;
136 MMAL_CLOCK_UPDATE_THRESHOLD_T update_threshold;
137 MMAL_CLOCK_DISCONT_THRESHOLD_T discont_threshold;
138 MMAL_CLOCK_REQUEST_THRESHOLD_T request_threshold;
139
140 /** Clock port events */
141 struct
142 {
143 MMAL_LIST_T* queue; /**< pending events */
144 MMAL_LIST_T* free; /**< available event slots */
145 CLOCK_PORT_EVENT_T pool[MAX_CLOCK_EVENT_SLOTS];
146 } events;
147} MMAL_COMPONENT_MODULE_T;
148
149/** Clock port context */
150typedef struct MMAL_PORT_MODULE_T
151{
152 CLOCK_STREAM_T *stream; /**< stream associated with this clock port */
153} MMAL_PORT_MODULE_T;
154
155
156/*****************************************************************************/
157/** Round x up to the next power of two */
158static uint32_t next_pow2(uint32_t x)
159{
160 x--;
161 x = (x >> 1) | x;
162 x = (x >> 2) | x;
163 x = (x >> 4) | x;
164 x = (x >> 8) | x;
165 x = (x >> 16) | x;
166 return ++x;
167}
168
169/** Given a power of 2 value, return the number of bit shifts */
170static uint32_t pow2_shift(uint32_t x)
171{
172 static const uint32_t BIT_POSITIONS[32] =
173 {
174 0, 1, 28, 2, 29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4, 8,
175 31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6, 11, 5, 10, 9
176 };
177
178 return BIT_POSITIONS[((x & -x) * 0x077CB531U) >> 27];
179}
180
181/** Add 2 values with saturation */
182static inline TIME_T saturate_add(TIME_T a, TIME_T b)
183{
184 TIME_T sum = a + b;
185 if (a > 0 && b > 0 && sum < 0)
186 sum = MAX_TIME;
187 else if (a < 0 && b < 0 && sum > 0)
188 sum = MIN_TIME;
189 return sum;
190}
191
192/*****************************************************************************/
193/** Filter reset */
194static void filter_init(FILTER_T *filter, uint32_t length)
195{
196 memset(filter, 0, sizeof(*filter));
197 filter->last = length - 1;
198 filter->length = length;
199};
200
201/** Increment filter index modulo the length */
202static inline uint32_t filter_index_wrap(uint32_t index, uint32_t length)
203{
204 return (++index < length) ? index : 0;
205}
206
207/** Remove the oldest sample from the filter */
208static void filter_drop(FILTER_T *filter)
209{
210 if (!filter->count)
211 return;
212
213 filter->sum -= filter->h[filter->first];
214 filter->first = filter_index_wrap(filter->first, filter->length);
215 filter->count--;
216}
217
218/** Add a new sample (and drop the oldest when full) */
219static void filter_insert(FILTER_T *filter, TIME_T sample)
220{
221 if (filter->count == filter->length)
222 filter_drop(filter);
223
224 filter->last = filter_index_wrap(filter->last, filter->length);
225 filter->h[filter->last] = sample;
226 filter->sum = saturate_add(filter->sum, sample);
227 filter->count++;
228}
229
230/*****************************************************************************/
231/** Create and initialise a clock stream */
232static MMAL_BOOL_T clock_create_stream(CLOCK_STREAM_T **stream, uint32_t id, uint32_t filter_length)
233{
234 CLOCK_STREAM_T *s = vcos_calloc(1, sizeof(CLOCK_STREAM_T), "clock stream");
235 if (!s)
236 {
237 LOG_ERROR("failed to allocate stream");
238 return MMAL_FALSE;
239 }
240
241 s->id = id;
242
243 filter_init(&s->avg_filter, filter_length);
244 filter_init(&s->std_filter, filter_length);
245
246 *stream = s;
247 return MMAL_TRUE;
248}
249
250/** Flag this stream as started */
251static void clock_start_stream(CLOCK_STREAM_T *stream, TIME_T stc, TIME_T pts)
252{
253 stream->started = MMAL_TRUE;
254 stream->pts = pts;
255 stream->stc = stc;
256}
257
258/** Reset the internal state of a stream */
259static void clock_reset_stream(CLOCK_STREAM_T *stream)
260{
261 if (!stream)
262 return;
263
264 stream->pts = 0;
265 stream->stc = 0;
266 stream->mt_off = 0;
267 stream->mt_off_avg = 0;
268 stream->mt_off_std = 0;
269 stream->started = MMAL_FALSE;
270
271 filter_init(&stream->avg_filter, stream->avg_filter.length);
272 filter_init(&stream->std_filter, stream->std_filter.length);
273}
274
275/** Update the internal state of a stream */
276static CLOCK_STREAM_EVENT_T clock_update_stream(CLOCK_STREAM_T *stream, TIME_T stc, TIME_T pts,
277 TIME_T discont_threshold)
278{
279 CLOCK_STREAM_EVENT_T event = CLOCK_STREAM_EVENT_NONE;
280 TIME_T pts_delta, stc_delta;
281
282 if (pts == MMAL_TIME_UNKNOWN)
283 {
284 LOG_TRACE("ignoring invalid timestamp received at %"PRIi64, stc);
285 return CLOCK_STREAM_EVENT_NONE;
286 }
287
288 if (!stream->started)
289 {
290 LOG_TRACE("stream %d started %"PRIi64" %"PRIi64, stream->id, stc, pts);
291 clock_start_stream(stream, stc, pts);
292 return CLOCK_STREAM_EVENT_STARTED;
293 }
294
295 /* XXX: This should really use the buffer flags to determine if a complete
296 * frame has been received. However, not all clients set MMAL buffer flags
297 * correctly (if at all). */
298 pts_delta = pts - stream->pts;
299 stc_delta = stc - stream->stc;
300
301 /* Check for discontinuities. */
302 if ((ABS(pts_delta) > discont_threshold) || (ABS(stc_delta) > discont_threshold))
303 {
304 LOG_ERROR("discontinuity detected on stream %d %"PRIi64" %"PRIi64" %"PRIi64,
305 stream->id, pts_delta, stc_delta, discont_threshold);
306 return CLOCK_STREAM_EVENT_DISCONT;
307 }
308
309 if (pts_delta)
310 {
311 /* A complete frame has now been received, so update the stream's notion of media time */
312 stream->mt_off = stream->pts - stream->stc;
313
314 filter_insert(&stream->avg_filter, stream->mt_off);
315 stream->mt_off_avg = stream->avg_filter.sum / stream->avg_filter.count;
316
317 filter_insert(&stream->std_filter, ABS(stream->mt_off - stream->mt_off_avg));
318 stream->mt_off_std = stream->std_filter.sum / stream->std_filter.count;
319
320 LOG_TRACE("stream %d %"PRIi64" %"PRIi64" %"PRIi64" %"PRIi64,
321 stream->id, stream->stc, stream->pts, stream->mt_off_avg, stream->mt_off);
322
323 event = CLOCK_STREAM_EVENT_FRAME_COMPLETE;
324 }
325
326 stream->pts = pts;
327 stream->stc = stc;
328
329 return event;
330}
331
332/*****************************************************************************/
333/** Start all enabled clock ports, making sure all use the same thresholds */
334static void clock_start_clocks(MMAL_COMPONENT_T *component, TIME_T media_time)
335{
336 MMAL_COMPONENT_MODULE_T *module = component->priv->module;
337 unsigned i;
338
339 for (i = 0; i < component->clock_num; ++i)
340 {
341 MMAL_PORT_T *port = component->clock[i];
342 if (port->is_enabled)
343 {
344 LOG_TRACE("starting clock %d with time %"PRIi64, port->index, media_time);
345 mmal_port_clock_reference_set(port, MMAL_TRUE);
346 mmal_port_clock_media_time_set(port, media_time);
347 mmal_port_clock_update_threshold_set(port, &module->update_threshold);
348 mmal_port_clock_discont_threshold_set(port, &module->discont_threshold);
349 mmal_port_clock_request_threshold_set(port, &module->request_threshold);
350 mmal_port_clock_active_set(port, MMAL_TRUE);
351 }
352 }
353}
354
355/** Stop (and flush) all enabled clock ports */
356static void clock_stop_clocks(MMAL_COMPONENT_T *component)
357{
358 unsigned i;
359
360 for (i = 0; i < component->clock_num; ++i)
361 {
362 MMAL_PORT_T *port = component->clock[i];
363 if (port->is_enabled)
364 {
365 LOG_TRACE("stopping clock %d", port->index);
366 mmal_port_clock_request_flush(port);
367 mmal_port_clock_active_set(port, MMAL_FALSE);
368 }
369 }
370}
371
372/** Reset the internal state of all streams in order to rebase clock
373 * adjustment calculations */
374static void clock_reset_clocks(MMAL_COMPONENT_T *component)
375{
376 MMAL_COMPONENT_MODULE_T *module = component->priv->module;
377 unsigned i;
378
379 for (i = 0; i < component->clock_num; ++i)
380 clock_reset_stream(component->clock[i]->priv->module->stream);
381
382 module->clock_discont = MMAL_TRUE;
383}
384
385/** Change the media-time for all enabled clock ports */
386static void clock_set_media_time(MMAL_COMPONENT_T *component, TIME_T media_time)
387{
388 unsigned i;
389
390 for (i = 0; i < component->clock_num; ++i)
391 {
392 MMAL_PORT_T *port = component->clock[i];
393 if (port->is_enabled)
394 mmal_port_clock_media_time_set(port, media_time);
395 }
396}
397
398/** Change the scale for all clock ports */
399static void clock_set_scale(MMAL_COMPONENT_T *component, MMAL_RATIONAL_T scale)
400{
401 unsigned i;
402
403 for (i = 0; i < component->clock_num; ++i)
404 mmal_port_clock_scale_set(component->clock[i], scale);
405
406 component->priv->module->pending_scale = 0;
407}
408
409/** Update the average and standard deviation calculations for all streams
410 * (dropping samples where necessary) and return the minimum and maximum
411 * streams */
412static MMAL_BOOL_T clock_get_mt_off_avg(MMAL_COMPONENT_T *component, TIME_T stc,
413 CLOCK_STREAM_T **minimum, CLOCK_STREAM_T **maximum)
414{
415 MMAL_COMPONENT_MODULE_T *module = component->priv->module;
416 TIME_T drop_threshold = 6 * module->frame_duration;
417 TIME_T reset_threshold = module->latency.target << 1;
418 TIME_T avg_min = MAX_TIME;
419 TIME_T avg_max = MIN_TIME;
420 TIME_T avg_bias;
421 TIME_T stc_delta;
422 unsigned i;
423
424 *minimum = 0;
425 *maximum = 0;
426
427 for (i = 0; i < component->clock_num; ++i)
428 {
429 CLOCK_STREAM_T *stream = component->clock[i]->priv->module->stream;
430 if (stream)
431 {
432 stc_delta = stc - stream->stc;
433
434 /* Drop samples from the moving average and standard deviation filters */
435 if (stc_delta > reset_threshold)
436 {
437 filter_init(&stream->avg_filter, stream->avg_filter.length);
438 filter_init(&stream->std_filter, stream->std_filter.length);
439 LOG_TRACE("reset stream %d filters due to stc_delta %"PRIi64, stream->id, stc_delta);
440 }
441 else if (stc_delta > drop_threshold)
442 {
443 filter_drop(&stream->avg_filter);
444 filter_drop(&stream->std_filter);
445 LOG_TRACE("drop stream %d filter samples due to stc_delta %"PRIi64, stream->id, stc_delta);
446 }
447
448 /* No point in continuing if filters are empty */
449 if (!stream->avg_filter.count)
450 continue;
451
452 /* Calculate new average and standard deviation for the stream */
453 stream->mt_off_avg = stream->avg_filter.sum / stream->avg_filter.count;
454 stream->mt_off_std = stream->std_filter.sum / stream->std_filter.count;
455
456 /* Select the minimum and maximum average between all active streams */
457 avg_bias = (stream->avg_filter.length - stream->avg_filter.count) * ABS(stream->mt_off_avg) / stream->avg_filter.length;
458 if ((stream->mt_off_avg + avg_bias) < avg_min)
459 {
460 avg_min = stream->mt_off_avg;
461 *minimum = stream;
462 LOG_TRACE("found min on %d mt_off_avg %"PRIi64" mt_off_std %"PRIi64" avg_bias %"PRIi64" count %d",
463 stream->id, stream->mt_off_avg, stream->mt_off_std, avg_bias, stream->avg_filter.count);
464 }
465 if ((stream->mt_off_avg - avg_bias) > avg_max)
466 {
467 avg_max = stream->mt_off_avg;
468 *maximum = stream;
469 LOG_TRACE("found max on %d mt_off_avg %"PRIi64" mt_off_std %"PRIi64" avg_bias %"PRIi64" count %d",
470 stream->id, stream->mt_off_avg, stream->mt_off_std, avg_bias, stream->avg_filter.count);
471 }
472 }
473 }
474
475 return (*minimum) && (*maximum);
476}
477
478/** Adjust the media-time of the playback clocks based on current timing statistics */
479static void clock_adjust_clocks(MMAL_COMPONENT_T *component, TIME_T stc)
480{
481 MMAL_COMPONENT_MODULE_T *module = component->priv->module;
482 CLOCK_STREAM_T *stream_min;
483 CLOCK_STREAM_T *stream_max;
484 TIME_T mt_off_clk;
485 TIME_T stc_prev;
486
487 if (!clock_get_mt_off_avg(component, stc, &stream_min, &stream_max))
488 return;
489
490 module->stream_min_id = stream_min->id;
491 module->stream_max_id = stream_max->id;
492
493 /* Calculate the actual media-time offset seen by the clock */
494 mt_off_clk = mmal_port_clock_media_time_get(component->clock[0]) - stc;
495
496 stc_prev = module->stc_at_update;
497 module->stc_at_update = stc;
498
499 /* If there has been a discontinuity, restart the clock,
500 * else use the clock control loop to apply a clock adjustment */
501 if (module->clock_discont)
502 {
503 module->clock_discont = MMAL_FALSE;
504
505 module->mt_off_clk = stream_min->mt_off_avg - module->latency.target;
506 module->mt_off_target = module->mt_off_clk;
507
508 clock_stop_clocks(component);
509 clock_start_clocks(component, module->mt_off_clk + stc);
510 }
511 else
512 {
513 /* Determine the new clock target */
514 TIME_T mt_off_target_max = stream_max->mt_off_avg - module->latency.target;
515 TIME_T mt_off_target_min = stream_min->mt_off_avg - module->frame_duration;
516 module->mt_off_target = MIN(mt_off_target_max, mt_off_target_min);
517
518 /* Calculate the proportional adjustment, capped by the attack rate
519 * set by the client */
520 TIME_T stc_delta = (stc > stc_prev) ? (stc - stc_prev) : 0;
521 TIME_T adj_p_max = stc_delta * module->latency.attack_rate / module->latency.attack_period;
522
523 module->adj_p = module->mt_off_target - module->mt_off_clk;
524 if (module->adj_p < -adj_p_max)
525 module->adj_p = -adj_p_max;
526 else if (module->adj_p > adj_p_max)
527 module->adj_p = adj_p_max;
528
529 /* Calculate the confidence of the adjustment using the approximate
530 * standard deviation for the selected stream:
531 *
532 * adj_m = 1.0 - STD * FPS / 4
533 *
534 * The adjustment factor is scaled up by 2^20 which is an approximation
535 * of 1000000 (microseconds per second) and the frame rate is assumed
536 * to be either 32 or 64 which are approximations for 24/25/30 and 60
537 * fps to avoid divisions. This has a lower limit of 0. */
538 module->adj_m =
539 MAX((1 << 20) - ((stream_min->mt_off_std << module->frame_rate_log2) >> 2), 0);
540
541 /* Modulate the proportional adjustment by the sample confidence
542 * and apply the adjustment to the current clock */
543 module->adj = (module->adj_p * module->adj_m) >> 20;
544 module->adj = (module->adj * (stream_min->avg_filter.count << 8) / stream_min->avg_filter.length) >> 8;
545 module->mt_off_clk += module->adj;
546
547 clock_set_media_time(component, module->mt_off_clk + stc);
548 }
549
550 /* Any pending clock scale changes can now be applied */
551 if (component->priv->module->pending_scale)
552 clock_set_scale(component, component->priv->module->scale);
553}
554
555/*****************************************************************************/
556static void clock_process_stream_event(MMAL_COMPONENT_T *component, CLOCK_STREAM_T *stream,
557 CLOCK_STREAM_EVENT_T event, TIME_T stc)
558{
559 MMAL_COMPONENT_MODULE_T *module = component->priv->module;
560
561 switch (event)
562 {
563 case CLOCK_STREAM_EVENT_FRAME_COMPLETE:
564 clock_adjust_clocks(component, stc);
565 if (clock_additional_logging)
566 {
567 VCOS_ALERT("STRM_%d %"PRIi64" %"PRIi64" %"PRIi64" %"PRIi64" %"PRIi64" %"PRIi64" %d %"
568 PRIi64" %"PRIi64" %"PRIi64" %"PRIi64" %"PRIi64" %u %u",
569 stream->id, stream->stc, stream->pts, stream->mt_off_avg, stream->mt_off,
570 stream->mt_off_std, ABS(stream->mt_off - stream->mt_off_avg), stream->avg_filter.count,
571 module->mt_off_clk, module->mt_off_target, module->adj_p, module->adj_m, module->adj,
572 module->stream_min_id, module->stream_max_id);
573 }
574 break;
575 case CLOCK_STREAM_EVENT_DISCONT:
576 clock_reset_clocks(component);
577 break;
578 default:
579 /* ignore all other events */
580 break;
581 }
582}
583
584/** Handler for input buffer events */
585static void clock_process_input_buffer_info_event(MMAL_COMPONENT_T *component, MMAL_PORT_T *port,
586 const MMAL_CLOCK_BUFFER_INFO_T *info)
587{
588 CLOCK_STREAM_EVENT_T stream_event = CLOCK_STREAM_EVENT_NONE;
589 MMAL_COMPONENT_MODULE_T *module = component->priv->module;
590 MMAL_PORT_MODULE_T *port_module = port->priv->module;
591 TIME_T stc = (TIME_T)((uint64_t)info->arrival_time);
592 TIME_T pts = info->time_stamp;
593
594 LOG_TRACE("port %d %"PRIi64" %"PRIi64, port->index, stc, pts);
595
596 if (!port_module->stream)
597 {
598 /* First data received for this stream */
599 uint32_t filter_length = module->frame_rate.num * FILTER_DURATION /
600 module->frame_rate.den;
601 if (!clock_create_stream(&port_module->stream, port->index, filter_length))
602 return;
603 }
604
605 stream_event = clock_update_stream(port_module->stream, stc, pts, module->discont_threshold.threshold);
606
607 clock_process_stream_event(component, port_module->stream, stream_event, stc);
608}
609
610/** Handler for clock scale events */
611static void clock_process_scale_event(MMAL_COMPONENT_T *component, MMAL_RATIONAL_T scale)
612{
613 /* When pausing the clock (i.e. scale = 0.0), apply the scale change
614 * immediately. However, when resuming the clock (i.e. scale = 1.0),
615 * the scale change can only be applied the next time new buffer timing
616 * information is received. This ensures that clocks resume with the
617 * correct media-time. */
618 if (scale.num == 0)
619 {
620 component->priv->module->scale = scale;
621 clock_set_scale(component, scale);
622 }
623 else
624 {
625 /* Only support scale == 1.0 */
626 if (!mmal_rational_equal(component->priv->module->scale, scale) &&
627 (scale.num == scale.den))
628 {
629 component->priv->module->scale = scale;
630 component->priv->module->pending_scale = 1;
631 clock_reset_clocks(component);
632 }
633 }
634}
635
636/** Handler for update threshold events */
637static void clock_process_update_threshold_event(MMAL_COMPONENT_T *component, const MMAL_CLOCK_UPDATE_THRESHOLD_T *threshold)
638{
639 unsigned i;
640
641 component->priv->module->update_threshold = *threshold;
642
643 for (i = 0; i < component->clock_num; ++i)
644 mmal_port_clock_update_threshold_set(component->clock[i], threshold);
645}
646
647/** Handler for discontinuity threshold events */
648static void clock_process_discont_threshold_event(MMAL_COMPONENT_T *component, const MMAL_CLOCK_DISCONT_THRESHOLD_T *threshold)
649{
650 unsigned i;
651
652 component->priv->module->discont_threshold = *threshold;
653
654 for (i = 0; i < component->clock_num; ++i)
655 mmal_port_clock_discont_threshold_set(component->clock[i], threshold);
656}
657
658/** Handler for request threshold events */
659static void clock_process_request_threshold_event(MMAL_COMPONENT_T *component, const MMAL_CLOCK_REQUEST_THRESHOLD_T *threshold)
660{
661 unsigned i;
662
663 component->priv->module->request_threshold = *threshold;
664
665 for (i = 0; i < component->clock_num; ++i)
666 mmal_port_clock_request_threshold_set(component->clock[i], threshold);
667}
668
669/** Handler for latency events */
670static void clock_process_latency_event(MMAL_COMPONENT_T *component, const MMAL_CLOCK_LATENCY_T *latency)
671{
672 component->priv->module->latency = *latency;
673
674 clock_reset_clocks(component);
675}
676
677/** Add a clock port event to the queue and trigger the action thread */
678static MMAL_STATUS_T clock_event_queue(MMAL_COMPONENT_T *component, MMAL_PORT_T *port, const MMAL_CLOCK_EVENT_T *event)
679{
680 CLOCK_PORT_EVENT_T *slot = (CLOCK_PORT_EVENT_T*)mmal_list_pop_front(component->priv->module->events.free);
681 if (!slot)
682 {
683 LOG_ERROR("no event slots available");
684 return MMAL_ENOSPC;
685 }
686
687 slot->port = port;
688 slot->event = *event;
689 mmal_list_push_back(component->priv->module->events.queue, &slot->link);
690
691 return mmal_component_action_trigger(component);
692}
693
694/** Get the next clock port event in the queue */
695static MMAL_STATUS_T clock_event_dequeue(MMAL_COMPONENT_T *component, CLOCK_PORT_EVENT_T *port_event)
696{
697 CLOCK_PORT_EVENT_T *slot = (CLOCK_PORT_EVENT_T*)mmal_list_pop_front(component->priv->module->events.queue);
698 if (!slot)
699 return MMAL_EINVAL;
700
701 port_event->port = slot->port;
702 port_event->event = slot->event;
703 mmal_list_push_back(component->priv->module->events.free, &slot->link);
704
705 if (port_event->event.buffer)
706 {
707 port_event->event.buffer->length = 0;
708 mmal_port_buffer_header_callback(port_event->port, port_event->event.buffer);
709 }
710
711 return MMAL_SUCCESS;
712}
713
714/** Event callback from a clock port */
715static void clock_event_cb(MMAL_PORT_T *port, const MMAL_CLOCK_EVENT_T *event)
716{
717 clock_event_queue(port->component, port, event);
718}
719
720
721/*****************************************************************************/
722/** Actual processing function */
723static MMAL_BOOL_T clock_do_processing(MMAL_COMPONENT_T *component)
724{
725 CLOCK_PORT_EVENT_T port_event;
726
727 if (clock_event_dequeue(component, &port_event) != MMAL_SUCCESS)
728 return MMAL_FALSE; /* No more external events to process */
729
730 /* Process external events (coming from clock ports) */
731 switch (port_event.event.id)
732 {
733 case MMAL_CLOCK_EVENT_SCALE:
734 clock_process_scale_event(component, port_event.event.data.scale);
735 break;
736 case MMAL_CLOCK_EVENT_UPDATE_THRESHOLD:
737 clock_process_update_threshold_event(component, &port_event.event.data.update_threshold);
738 break;
739 case MMAL_CLOCK_EVENT_DISCONT_THRESHOLD:
740 clock_process_discont_threshold_event(component, &port_event.event.data.discont_threshold);
741 break;
742 case MMAL_CLOCK_EVENT_REQUEST_THRESHOLD:
743 clock_process_request_threshold_event(component, &port_event.event.data.request_threshold);
744 break;
745 case MMAL_CLOCK_EVENT_LATENCY:
746 clock_process_latency_event(component, &port_event.event.data.latency);
747 break;
748 case MMAL_CLOCK_EVENT_INPUT_BUFFER_INFO:
749 clock_process_input_buffer_info_event(component, port_event.port, &port_event.event.data.buffer);
750 break;
751 default:
752 break;
753 }
754
755 return MMAL_TRUE;
756}
757
758/** Component action thread */
759static void clock_do_processing_loop(MMAL_COMPONENT_T *component)
760{
761 while (clock_do_processing(component));
762}
763
764
765/*****************************************************************************/
766/** Set a parameter on the clock component's control port */
767static MMAL_STATUS_T clock_control_parameter_set(MMAL_PORT_T *port, const MMAL_PARAMETER_HEADER_T *param)
768{
769 MMAL_COMPONENT_T *component = port->component;
770 MMAL_COMPONENT_MODULE_T *module = component->priv->module;
771 MMAL_STATUS_T status = MMAL_SUCCESS;
772
773 switch (param->id)
774 {
775 case MMAL_PARAMETER_CLOCK_FRAME_RATE:
776 {
777 const MMAL_PARAMETER_FRAME_RATE_T *p = (const MMAL_PARAMETER_FRAME_RATE_T *)param;
778 module->frame_rate = p->frame_rate;
779 /* XXX: take frame_rate.den into account */
780 module->frame_rate_log2 = pow2_shift(next_pow2(module->frame_rate.num));
781 module->frame_duration = p->frame_rate.den * 1000000 / p->frame_rate.num;
782 LOG_TRACE("frame rate %d/%d (%u) duration %"PRIi64,
783 module->frame_rate.num, module->frame_rate.den,
784 module->frame_rate_log2, module->frame_duration);
785 }
786 break;
787 case MMAL_PARAMETER_CLOCK_LATENCY:
788 {
789 /* Changing the latency setting requires a reset of the clock algorithm, but
790 * that can only be safely done from within the component's worker thread.
791 * So, queue the new latency setting as a clock event. */
792 const MMAL_PARAMETER_CLOCK_LATENCY_T *p = (const MMAL_PARAMETER_CLOCK_LATENCY_T *)param;
793 MMAL_CLOCK_EVENT_T event = { MMAL_CLOCK_EVENT_LATENCY, MMAL_CLOCK_EVENT_MAGIC };
794
795 LOG_TRACE("latency target %"PRIi64" attack %"PRIi64"/%"PRIi64,
796 p->value.target, p->value.attack_rate, p->value.attack_period);
797
798 event.data.latency = p->value;
799 status = clock_event_queue(port->component, port, &event);
800 }
801 break;
802 default:
803 LOG_ERROR("parameter not supported (0x%x)", param->id);
804 status = MMAL_ENOSYS;
805 break;
806 }
807 return status;
808}
809
810/** Destroy a previously created component */
811static MMAL_STATUS_T clock_component_destroy(MMAL_COMPONENT_T *component)
812{
813 MMAL_COMPONENT_MODULE_T *module = component->priv->module;
814 unsigned int i;
815
816 if (module->events.free)
817 mmal_list_destroy(module->events.free);
818
819 if (module->events.queue)
820 mmal_list_destroy(module->events.queue);
821
822 if (component->clock_num)
823 {
824 for (i = 0; i < component->clock_num; ++i)
825 vcos_free(component->clock[i]->priv->module->stream);
826
827 mmal_ports_clock_free(component->clock, component->clock_num);
828 }
829
830 vcos_free(module);
831
832 return MMAL_SUCCESS;
833}
834
835/** Create an instance of a clock component */
836static MMAL_STATUS_T mmal_component_create_clock(const char *name, MMAL_COMPONENT_T *component)
837{
838 int i;
839 MMAL_COMPONENT_MODULE_T *module;
840 MMAL_STATUS_T status = MMAL_ENOMEM;
841 MMAL_PARAM_UNUSED(name);
842
843 /* Allocate the context for our module */
844 component->priv->module = module = vcos_malloc(sizeof(*module), "mmal module");
845 if (!module)
846 return MMAL_ENOMEM;
847 memset(module, 0, sizeof(*module));
848
849 component->priv->pf_destroy = clock_component_destroy;
850
851 /* Create the clock ports (clock ports are managed by the framework) */
852 component->clock = mmal_ports_clock_alloc(component, CLOCK_PORTS_NUM,
853 sizeof(MMAL_PORT_MODULE_T), clock_event_cb);
854 if (!component->clock)
855 goto error;
856 component->clock_num = CLOCK_PORTS_NUM;
857
858 component->control->priv->pf_parameter_set = clock_control_parameter_set;
859
860 /* Setup event slots */
861 module->events.free = mmal_list_create();
862 module->events.queue = mmal_list_create();
863 if (!module->events.free || !module->events.queue)
864 {
865 LOG_ERROR("failed to create list %p %p", module->events.free, module->events.queue);
866 goto error;
867 }
868 for (i = 0; i < MAX_CLOCK_EVENT_SLOTS; ++i)
869 mmal_list_push_back(module->events.free, &module->events.pool[i].link);
870
871 component->priv->priority = VCOS_THREAD_PRI_REALTIME;
872 status = mmal_component_action_register(component, clock_do_processing_loop);
873
874 module->clock_discont = MMAL_TRUE;
875 module->frame_rate.num = DEFAULT_FRAME_RATE;
876 module->frame_rate.den = 1;
877
878 module->scale = mmal_port_clock_scale_get(component->clock[0]);
879
880 memset(&module->latency, 0, sizeof(module->latency));
881 module->latency.target = DEFAULT_CLOCK_LATENCY;
882
883 mmal_port_clock_update_threshold_get(component->clock[0], &module->update_threshold);
884 mmal_port_clock_discont_threshold_get(component->clock[0], &module->discont_threshold);
885 mmal_port_clock_request_threshold_get(component->clock[0], &module->request_threshold);
886
887 return status;
888
889 error:
890 clock_component_destroy(component);
891 return status;
892}
893
894
895/*****************************************************************************/
896MMAL_CONSTRUCTOR(mmal_register_component_clock);
897void mmal_register_component_clock(void)
898{
899 mmal_component_supplier_register("clock", mmal_component_create_clock);
900}
901