1 /*
2 * MPEGTS functions
3 * Copyright (C) 2010-2011 Unix Solutions Ltd.
4 *
5 * Released under MIT license.
6 * See LICENSE-MIT.txt for license terms.
7 */
8 #include <stdio.h>
9 #include <unistd.h>
10 #include <netdb.h>
11 #include <stdlib.h>
12 #include <string.h>
13 #include <ctype.h>
14 #include <sys/time.h>
15
16 #include "tsfuncs.h"
17
ts_packet_init_null(uint8_t * ts_packet)18 void ts_packet_init_null(uint8_t *ts_packet) {
19 memset(ts_packet, 0xff, TS_PACKET_SIZE);
20 ts_packet[0] = 0x47;
21 ts_packet[1] = 0x1f;
22 ts_packet[2] = 0xff;
23 ts_packet[3] = 0x00;
24 }
25
ts_packet_set_scrambled(uint8_t * ts_packet,enum ts_scrambled_type stype)26 void ts_packet_set_scrambled(uint8_t *ts_packet, enum ts_scrambled_type stype) {
27 ts_packet_set_not_scrambled(ts_packet);
28 if (stype == scrambled_with_even_key)
29 ts_packet[3] |= 2 << 6;
30 if (stype == scrambled_with_odd_key)
31 ts_packet[3] |= 3 << 6;
32 if (stype == scrambled_reserved)
33 ts_packet[3] |= 1 << 6;
34 }
35
ts_packet_get_payload_offset(uint8_t * ts_packet)36 uint8_t ts_packet_get_payload_offset(uint8_t *ts_packet) {
37 if (ts_packet[0] != 0x47)
38 return 0;
39
40 uint8_t adapt_field = (ts_packet[3] &~ 0xDF) >> 5; // 11x11111
41 uint8_t payload_field = (ts_packet[3] &~ 0xEF) >> 4; // 111x1111
42
43 if (!adapt_field && !payload_field) // Not allowed
44 return 0;
45
46 if (adapt_field) {
47 uint8_t adapt_len = ts_packet[4];
48 if (payload_field && adapt_len > 182) // Validity checks
49 return 0;
50 if (!payload_field && adapt_len > 183)
51 return 0;
52 if (adapt_len + 4 > 188) // adaptation field takes the whole packet
53 return 0;
54 return 4 + 1 + adapt_len; // ts header + adapt_field_len_byte + adapt_field_len
55 } else {
56 return 4; // No adaptation, data starts directly after TS header
57 }
58 }
59
ts_decode_pts_dts(uint8_t * data,uint64_t * value)60 void ts_decode_pts_dts(uint8_t *data, uint64_t *value) {
61 uint64_t pts1 = ((unsigned int)data[0] & 0x0E) >> 1;
62 uint64_t pts2 = ((unsigned int)data[1] << 7) | (((unsigned int)data[2] & 0xFE) >> 1);
63 uint64_t pts3 = ((unsigned int)data[3] << 7) | (((unsigned int)data[4] & 0xFE) >> 1);
64 *value = (pts1 << 30) | (pts2 << 15) | pts3;
65 }
66
67 /*
68 * guard 2 == pts
69 * guard 3 == pts before dts
70 * guard 1 == dts
71 */
ts_encode_pts_dts(uint8_t * data,int guard,uint64_t value)72 void ts_encode_pts_dts(uint8_t *data, int guard, uint64_t value) {
73 #define MAX_PTS_VALUE 0x1FFFFFFFFLL
74 while (value > MAX_PTS_VALUE)
75 value -= MAX_PTS_VALUE;
76
77 unsigned int pts1 = (unsigned int)((value >> 30) & 0x07);
78 unsigned int pts2 = (unsigned int)((value >> 15) & 0x7FFF);
79 unsigned int pts3 = (unsigned int)( value & 0x7FFF);
80
81 data[0] = (guard << 4) | (pts1 << 1) | 0x01;
82 data[1] = (pts2 & 0x7F80) >> 7;
83 data[2] = ((pts2 & 0x007F) << 1) | 0x01;
84 data[3] = (pts3 & 0x7F80) >> 7;
85 data[4] = ((pts3 & 0x007F) << 1) | 0x01;
86 }
87
88 // Return 0 on failure
89 // Return payload_ofs on success
ts_packet_has_pes(uint8_t * ts_packet,uint16_t * pes_packet_len)90 int ts_packet_has_pes(uint8_t *ts_packet, uint16_t *pes_packet_len) {
91 uint8_t payload_ofs;
92 uint8_t *payload;
93
94 if (!ts_packet_is_pusi(ts_packet))
95 goto ERR;
96
97 payload_ofs = ts_packet_get_payload_offset(ts_packet);
98 if (!payload_ofs)
99 goto ERR;
100
101 if (payload_ofs + 6 + 2 >= 188) // 6 bytes pes header, 2 bytes pes flags
102 goto ERR;
103
104 payload = ts_packet + payload_ofs;
105 if (payload[0] == 0x00 && payload[1] == 0x00 && payload[2] == 0x01) { // pes_start_code_prefix
106 uint8_t stream_id = payload[3];
107 if (pes_packet_len)
108 *pes_packet_len = (payload[4] << 8) | payload[5];
109 // We do not handle this streams...
110 if (!IS_PES_STREAM_SUPPORTED(stream_id))
111 goto ERR;
112 return payload_ofs;
113 }
114
115 ERR:
116 return 0;
117 }
118
ts_packet_has_pts_dts(uint8_t * ts_packet,uint64_t * pts,uint64_t * dts)119 int ts_packet_has_pts_dts(uint8_t *ts_packet, uint64_t *pts, uint64_t *dts) {
120 *pts = NO_PTS;
121 *dts = NO_DTS;
122
123 uint8_t payload_ofs = ts_packet_has_pes(ts_packet, NULL);
124 if (!payload_ofs)
125 goto ERR;
126
127 uint8_t *data = ts_packet + payload_ofs;
128 uint8_t *data_end = ts_packet + 188;
129 if ((data[6] &~ 0x3f) != 0x80) // 10xxxxxx (first two bits must be 10 eq 0x80
130 goto ERR;
131
132 if (data + 7 >= data_end) goto ERR;
133 uint8_t pts_flag = bit_on(data[7], bit_8); // PES flags 2
134 uint8_t dts_flag = bit_on(data[7], bit_7); // PES flags 2
135
136 if (!pts_flag && dts_flag) // Invalid, can't have only DTS flag
137 return 0;
138
139 if (pts_flag && !dts_flag) {
140 if (data + 14 >= data_end) goto ERR;
141 ts_decode_pts_dts(&data[9], pts);
142 } else if (pts_flag && dts_flag) {
143 if (data + 19 >= data_end) goto ERR;
144 ts_decode_pts_dts(&data[9], pts);
145 ts_decode_pts_dts(&data[14], dts);
146 }
147 return 1;
148
149 ERR:
150 return 0;
151 }
152
ts_packet_change_pts(uint8_t * ts_packet,uint64_t pts)153 void ts_packet_change_pts(uint8_t *ts_packet, uint64_t pts) {
154 uint8_t payload_offset = ts_packet_get_payload_offset(ts_packet);
155 if (!payload_offset)
156 return;
157 uint8_t *data = ts_packet + payload_offset;
158 ts_encode_pts_dts(&data[9], 2, pts);
159 }
160
ts_packet_change_pts_dts(uint8_t * ts_packet,uint64_t pts,uint64_t dts)161 void ts_packet_change_pts_dts(uint8_t *ts_packet, uint64_t pts, uint64_t dts) {
162 uint8_t payload_offset = ts_packet_get_payload_offset(ts_packet);
163 if (!payload_offset)
164 return;
165 uint8_t *data = ts_packet + payload_offset;
166 ts_encode_pts_dts(&data[9], 3, pts);
167 ts_encode_pts_dts(&data[14], 1, dts);
168 }
169
170
ts_packet_has_pcr(uint8_t * ts_packet)171 int ts_packet_has_pcr(uint8_t *ts_packet) {
172 if (ts_packet[0] == 0x47) { // TS packet
173 if (bit_on(ts_packet[3], bit_6)) { // Packet have adaptation field
174 if (ts_packet[4] > 6) { // Adaptation field length is > 6
175 if (bit_on(ts_packet[5], bit_5)) { // The is PCR field
176 return 1;
177 } else {
178 // ts_LOGf("!no PCR field\n");
179 }
180 } else {
181 // ts_LOGf("!not enough adaptation len (%d), need at least 7\n", ts_packet[4]);
182 }
183 } else {
184 // ts_LOGf("!no adaptation field\n");
185 }
186 } else {
187 // ts_LOGf("!no ts packet start (0x%02x) need 0x47\n", ts_packet[0]);
188 }
189 return 0;
190 }
191
ts_packet_get_pcr_ex(uint8_t * ts_packet,uint64_t * pcr_base,uint16_t * pcr_ext)192 uint64_t ts_packet_get_pcr_ex(uint8_t *ts_packet, uint64_t *pcr_base, uint16_t *pcr_ext) {
193 uint8_t *data = ts_packet + 6; // Offset in TS packet
194 *pcr_base = (uint64_t)data[0] << 25;
195 *pcr_base += (uint64_t)data[1] << 17;
196 *pcr_base += (uint64_t)data[2] << 9;
197 *pcr_base += (uint64_t)data[3] << 1;
198 *pcr_base += (uint64_t)data[4] >> 7;
199 *pcr_ext = ((uint16_t)data[4] & 0x01) << 8;
200 *pcr_ext += (uint16_t)data[5];
201 //ts_LOGf("get pcr_base=%10llu pcr_ext=%4u pcr=%llu\n", *pcr_base, *pcr_ext, *pcr_base * 300ll + *pcr_ext);
202 return *pcr_base * 300ll + *pcr_ext;
203 }
204
ts_packet_get_pcr(uint8_t * ts_packet)205 uint64_t ts_packet_get_pcr(uint8_t *ts_packet) {
206 uint64_t pcr_base;
207 uint16_t pcr_ext;
208 return ts_packet_get_pcr_ex(ts_packet, &pcr_base, &pcr_ext);
209 }
210
211
ts_packet_set_pcr_ex(uint8_t * ts_packet,uint64_t pcr_base,uint16_t pcr_ext)212 void ts_packet_set_pcr_ex(uint8_t *ts_packet, uint64_t pcr_base, uint16_t pcr_ext) {
213 //ts_LOGf("set pcr_base=%10llu pcr_ext=%4u pcr=%llu\n", pcr_base, pcr_ext, pcr);
214 // 6 is the PCR offset in ts_packet (4 bytes header, 1 byte adapt field len)
215 ts_packet[6 + 0] = (pcr_base >> 25) & 0xFF;
216 ts_packet[6 + 1] = (pcr_base >> 17) & 0xFF;
217 ts_packet[6 + 2] = (pcr_base >> 9) & 0xFF;
218 ts_packet[6 + 3] = (pcr_base >> 1) & 0xFF;
219 ts_packet[6 + 4] = 0x7e | ((pcr_ext >> 8) & 0x01) | ((pcr_base & 0x01) <<7 ); // 0x7e == 6 reserved bits
220 ts_packet[6 + 5] = pcr_ext & 0xFF;
221 }
222
ts_packet_set_pcr(uint8_t * ts_packet,uint64_t pcr)223 void ts_packet_set_pcr(uint8_t *ts_packet, uint64_t pcr) {
224 uint64_t pcr_base = pcr / 300;
225 uint16_t pcr_ext = pcr % 300;
226 ts_packet_set_pcr_ex(ts_packet, pcr_base, pcr_ext);
227 }
228
ts_packet_header_parse(uint8_t * ts_packet,struct ts_header * ts_header)229 uint8_t *ts_packet_header_parse(uint8_t *ts_packet, struct ts_header *ts_header) {
230 if (ts_packet[0] != 0x47) {
231 // ts_LOGf("*** TS packet do not start with sync byte 0x47 but with 0x%02x\n", ts_packet[0]);
232 goto return_error;
233 }
234
235 ts_header->tei = (ts_packet[1] &~ 0x7f) >> 7; // x1111111
236 ts_header->pusi = (ts_packet[1] &~ 0xbf) >> 6; // 1x111111
237 ts_header->prio = (ts_packet[1] &~ 0xdf) >> 5; // 11x11111
238 ts_header->pid = (ts_packet[1] &~ 0xE0) << 8 | ts_packet[2]; // 111xxxxx xxxxxxxx
239
240 ts_header->scramble = (ts_packet[3] &~ 0x3F) >> 6; // xx111111
241 ts_header->adapt_field = (ts_packet[3] &~ 0xDF) >> 5; // 11x11111
242 ts_header->payload_field = (ts_packet[3] &~ 0xEF) >> 4; // 111x1111
243 ts_header->continuity = (ts_packet[3] &~ 0xF0); // 1111xxxx
244
245 if (!ts_header->adapt_field) {
246 ts_header->adapt_len = 0;
247 ts_header->adapt_flags = 0;
248 ts_header->payload_offset = 4;
249 } else {
250 ts_header->adapt_len = ts_packet[4];
251 if (ts_header->adapt_len) {
252 ts_header->adapt_flags = ts_packet[5];
253 }
254 ts_header->payload_offset = 5 + ts_header->adapt_len; // 2 bytes see above
255 }
256
257 if (!ts_header->adapt_field && !ts_header->payload_field) // Not allowed
258 goto return_error;
259
260 if (!ts_header->payload_field)
261 return NULL;
262
263 if (ts_header->payload_field && ts_header->adapt_len > 182) // Validity checks
264 goto return_error;
265 if (!ts_header->payload_field && ts_header->adapt_len > 183)
266 goto return_error;
267
268 if (ts_header->payload_offset > TS_MAX_PAYLOAD_SIZE) // Validity check
269 goto return_error;
270
271 ts_header->payload_size = TS_PACKET_SIZE - ts_header->payload_offset;
272
273 return ts_packet + ts_header->payload_offset;
274
275 return_error:
276 memset(ts_header, 0, sizeof(struct ts_header));
277 return NULL;
278 }
279
ts_packet_header_generate(uint8_t * ts_packet,struct ts_header * ts_header)280 void ts_packet_header_generate(uint8_t *ts_packet, struct ts_header *ts_header) {
281 memset(ts_packet, 0xFF, TS_PACKET_SIZE);
282 ts_packet[0] = 0x47;
283
284 ts_packet[1] = 0;
285 ts_packet[1] = ts_header->tei << 7; // x1111111
286 ts_packet[1] |= ts_header->pusi << 6; // 1x111111
287 ts_packet[1] |= ts_header->prio << 5; // 11x11111
288 ts_packet[1] |= ts_header->pid >> 8; // 111xxxxx xxxxxxxx
289 ts_packet[2] = ts_header->pid &~ 0xff00;
290
291 ts_packet[3] = 0;
292 ts_packet[3] = ts_header->scramble << 6; // xx111111
293 ts_packet[3] |= ts_header->adapt_field << 5; // 11x11111
294 ts_packet[3] |= ts_header->payload_field << 4; // 111x1111
295 ts_packet[3] |= ts_header->continuity; // 1111xxxx
296
297 if (ts_header->adapt_field) {
298 ts_packet[4] = ts_header->adapt_len;
299 ts_packet[5] = ts_header->adapt_flags;
300 }
301 }
302
ts_packet_header_dump(struct ts_header * ts_header)303 void ts_packet_header_dump(struct ts_header *ts_header) {
304 ts_LOGf("*** tei:%d pusi:%d prio:%d pid:%04x (%d) scramble:%d adapt:%d payload:%d adapt_len:%d adapt_flags:%d | pofs:%d plen:%d\n",
305 ts_header->tei,
306 ts_header->pusi,
307 ts_header->prio,
308 ts_header->pid,
309 ts_header->pid,
310 ts_header->scramble,
311 ts_header->adapt_field,
312 ts_header->payload_field,
313 ts_header->adapt_len,
314 ts_header->adapt_flags,
315 ts_header->payload_offset,
316 ts_header->payload_size
317 );
318 }
319