libtsfuncs is a library for mpeg PSI parsing and generation. https://georgi.unixsol.org/programs/libtsfuncs/

tsfuncs.c 13KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431
  1. #include <stdio.h>
  2. #include <unistd.h>
  3. #include <netdb.h>
  4. #include <stdlib.h>
  5. #include <string.h>
  6. #include <ctype.h>
  7. #include <sys/time.h>
  8. #include "tsfuncs.h"
  9. void ts_packet_init_null(uint8_t *ts_packet) {
  10. memset(ts_packet, 0xff, TS_PACKET_SIZE);
  11. ts_packet[0] = 0x47;
  12. ts_packet[1] = 0x1f;
  13. ts_packet[2] = 0xff;
  14. ts_packet[3] = 0x00;
  15. }
  16. inline int ts_packet_is_pusi(uint8_t *ts_packet) {
  17. return (ts_packet[1] &~ 0xbf) >> 6;
  18. }
  19. inline uint16_t ts_packet_get_pid(uint8_t *ts_packet) {
  20. return (ts_packet[1] &~ 0xE0) << 8 | ts_packet[2];
  21. }
  22. inline void ts_packet_set_pid(uint8_t *ts_packet, uint16_t new_pid) {
  23. ts_packet[1] = (ts_packet[1] &~ 0x1f) | (new_pid >> 8); // 111xxxxx xxxxxxxx
  24. ts_packet[2] = new_pid &~ 0xff00;
  25. }
  26. inline uint8_t ts_packet_get_cont(uint8_t *ts_packet) {
  27. return (ts_packet[3] &~ 0xF0); // 1111xxxx
  28. }
  29. inline void ts_packet_set_cont(uint8_t *ts_packet, uint8_t value) {
  30. // Mask the last 4 bits (continuity), then set the continuity
  31. ts_packet[3] = (ts_packet[3] &~ 0x0F) | (value &~ 0xF0);
  32. }
  33. inline void ts_packet_inc_cont(uint8_t *ts_packet, uint8_t increment) {
  34. ts_packet_set_cont(ts_packet, ts_packet_get_cont(ts_packet) + increment);
  35. }
  36. inline int ts_packet_get_scrambled(uint8_t *ts_packet) {
  37. return ts_packet[3] >> 6; // 0 is not scamlbed, 1 is reserved, 2 or 3 mean scrambled
  38. }
  39. inline int ts_packet_is_scrambled(uint8_t *ts_packet) {
  40. return ts_packet_get_scrambled(ts_packet) > 1;
  41. }
  42. inline void ts_packet_set_not_scrambled(uint8_t *ts_packet) {
  43. ts_packet[3] = ts_packet[3] &~ 0xc0; // Mask top two bits (11xxxxxx)
  44. }
  45. void ts_packet_set_scrambled(uint8_t *ts_packet, enum ts_scrambled_type stype) {
  46. ts_packet_set_not_scrambled(ts_packet);
  47. if (stype == scrambled_with_even_key)
  48. ts_packet[3] |= 2 << 6;
  49. if (stype == scrambled_with_odd_key)
  50. ts_packet[3] |= 3 << 6;
  51. if (stype == scrambled_reserved)
  52. ts_packet[3] |= 1 << 6;
  53. }
  54. uint8_t ts_packet_get_payload_offset(uint8_t *ts_packet) {
  55. if (ts_packet[0] != 0x47)
  56. return 0;
  57. uint8_t adapt_field = (ts_packet[3] &~ 0xDF) >> 5; // 11x11111
  58. uint8_t payload_field = (ts_packet[3] &~ 0xEF) >> 4; // 111x1111
  59. if (!adapt_field && !payload_field) // Not allowed
  60. return 0;
  61. if (adapt_field) {
  62. uint8_t adapt_len = ts_packet[4];
  63. if (payload_field && adapt_len > 182) // Validity checks
  64. return 0;
  65. if (!payload_field && adapt_len > 183)
  66. return 0;
  67. if (adapt_len + 1 + 4 >= 188) // adaptation field takes the whole packet
  68. return 0;
  69. return 4 + 1 + adapt_len; // ts header + adapt_field_len_byte + adapt_field_len
  70. } else {
  71. return 4; // No adaptation, data starts directly after TS header
  72. }
  73. }
  74. /*
  75. * Decode a PTS or DTS value.
  76. *
  77. * - `data` is the 5 bytes containing the encoded PTS or DTS value
  78. * - `required_guard` should be 2 for a PTS alone, 3 for a PTS before
  79. * a DTS, or 1 for a DTS after a PTS
  80. * - `value` is the PTS or DTS value as decoded
  81. *
  82. * Returns 0 if the PTS/DTS value is decoded successfully, 1 if an error occurs
  83. */
  84. int ts_decode_pts_dts(uint8_t *data, int required_guard, uint64_t *value) {
  85. uint64_t pts1,pts2,pts3;
  86. int marker;
  87. char *what;
  88. int guard = (data[0] & 0xF0) >> 4;
  89. // Rather than try to use casts to make the arithmetic come out right on both
  90. // Linux-with-gcc (old-style C rules) and Windows-with-VisualC++ (C99 rules),
  91. // it's simpler just to use intermediates that won't get cast to "int".
  92. unsigned int data0 = data[0];
  93. unsigned int data1 = data[1];
  94. unsigned int data2 = data[2];
  95. unsigned int data3 = data[3];
  96. unsigned int data4 = data[4];
  97. switch (required_guard) {
  98. case 2: what = "PTS"; break; // standalone
  99. case 3: what = "PTS"; break; // before a DTS
  100. case 1: what = "DTS"; break; // always after a PTS
  101. default: what = "???"; break;
  102. }
  103. if (guard != required_guard)
  104. {
  105. ts_LOGf("!!! decode_pts_dts(), Guard bits at start of %s data are %x, not %x\n", what, guard, required_guard);
  106. }
  107. pts1 = (data0 & 0x0E) >> 1;
  108. marker = data0 & 0x01;
  109. if (marker != 1)
  110. {
  111. ts_LOGf("!!! decode_pts_dts(), First %s marker is not 1\n",what);
  112. return 0;
  113. }
  114. pts2 = (data1 << 7) | ((data2 & 0xFE) >> 1);
  115. marker = data2 & 0x01;
  116. if (marker != 1)
  117. {
  118. ts_LOGf("!!! decode_pts_dts(), Second %s marker is not 1\n",what);
  119. return 0;
  120. }
  121. pts3 = (data3 << 7) | ((data4 & 0xFE) >> 1);
  122. marker = data4 & 0x01;
  123. if (marker != 1)
  124. {
  125. ts_LOGf("!!! decode_pts_dts(), Third %s marker is not 1\n",what);
  126. return 0;
  127. }
  128. *value = (pts1 << 30) | (pts2 << 15) | pts3;
  129. return 1;
  130. }
  131. /*
  132. * Encode a PTS or DTS.
  133. *
  134. * - `data` is the array of 5 bytes into which to encode the PTS/DTS
  135. * - `guard_bits` are the required guard bits: 2 for a PTS alone, 3 for
  136. * a PTS before a DTS, or 1 for a DTS after a PTS
  137. * - `value` is the PTS or DTS value to be encoded
  138. */
  139. void ts_encode_pts_dts(uint8_t *data, int guard_bits, uint64_t value) {
  140. int pts1,pts2,pts3;
  141. #define MAX_PTS_VALUE 0x1FFFFFFFFLL
  142. if (value > MAX_PTS_VALUE)
  143. {
  144. char *what;
  145. uint64_t temp = value;
  146. while (temp > MAX_PTS_VALUE)
  147. temp -= MAX_PTS_VALUE;
  148. switch (guard_bits)
  149. {
  150. case 2: what = "PTS alone"; break;
  151. case 3: what = "PTS before DTS"; break;
  152. case 1: what = "DTS after PTS"; break;
  153. default: what = "PTS/DTS/???"; break;
  154. }
  155. ts_LOGf("!!! value %llu for %s is more than %llu - reduced to %llu\n",value,what,MAX_PTS_VALUE,temp);
  156. value = temp;
  157. }
  158. pts1 = (int)((value >> 30) & 0x07);
  159. pts2 = (int)((value >> 15) & 0x7FFF);
  160. pts3 = (int)( value & 0x7FFF);
  161. data[0] = (guard_bits << 4) | (pts1 << 1) | 0x01;
  162. data[1] = (pts2 & 0x7F80) >> 7;
  163. data[2] = ((pts2 & 0x007F) << 1) | 0x01;
  164. data[3] = (pts3 & 0x7F80) >> 7;
  165. data[4] = ((pts3 & 0x007F) << 1) | 0x01;
  166. }
  167. // Return 0 on failure
  168. // Return payload_ofs on success
  169. int ts_packet_has_pes(uint8_t *ts_packet, uint16_t *pes_packet_len) {
  170. uint8_t payload_ofs;
  171. uint8_t *payload;
  172. if (!ts_packet_is_pusi(ts_packet))
  173. goto ERR;
  174. payload_ofs = ts_packet_get_payload_offset(ts_packet);
  175. if (!payload_ofs)
  176. goto ERR;
  177. if (payload_ofs + 6 + 2 >= 188) // 6 bytes pes header, 2 bytes pes flags
  178. goto ERR;
  179. payload = ts_packet + payload_ofs;
  180. if (payload[0] == 0x00 && payload[1] == 0x00 && payload[2] == 0x01) { // pes_start_code_prefix
  181. uint8_t stream_id = payload[3];
  182. if (pes_packet_len)
  183. *pes_packet_len = (payload[4] << 8) | payload[5];
  184. // We do not handle this streams...
  185. if (!IS_PES_STREAM_SUPPORTED(stream_id))
  186. goto ERR;
  187. return payload_ofs;
  188. }
  189. ERR:
  190. return 0;
  191. }
  192. int ts_packet_has_pts_dts(uint8_t *ts_packet, uint64_t *pts, uint64_t *dts) {
  193. *pts = NO_PTS;
  194. *dts = NO_DTS;
  195. uint8_t payload_ofs = ts_packet_has_pes(ts_packet, NULL);
  196. if (!payload_ofs)
  197. goto ERR;
  198. uint8_t *data = ts_packet + payload_ofs;
  199. uint8_t *data_end = ts_packet + 188;
  200. if ((data[6] &~ 0x3f) != 0x80) // 10xxxxxx (first two bits must be 10 eq 0x80
  201. goto ERR;
  202. if (data + 7 >= data_end) goto ERR;
  203. uint8_t pts_flag = bit_on(data[7], bit_8); // PES flags 2
  204. uint8_t dts_flag = bit_on(data[7], bit_7); // PES flags 2
  205. if (!pts_flag && dts_flag) // Invalid, can't have only DTS flag
  206. return 0;
  207. if (pts_flag && !dts_flag) {
  208. if (data + 14 >= data_end) goto ERR;
  209. if (!ts_decode_pts_dts(&data[9], 2, pts))
  210. goto ERR;
  211. } else if (pts_flag && dts_flag) {
  212. if (data + 19 >= data_end) goto ERR;
  213. if (!ts_decode_pts_dts(&data[9], 3, pts))
  214. goto ERR;
  215. if (!ts_decode_pts_dts(&data[14], 1, dts))
  216. goto ERR;
  217. }
  218. return 1;
  219. ERR:
  220. return 0;
  221. }
  222. void ts_packet_change_pts(uint8_t *ts_packet, uint64_t pts) {
  223. uint8_t payload_offset = ts_packet_get_payload_offset(ts_packet);
  224. if (!payload_offset)
  225. return;
  226. uint8_t *data = ts_packet + payload_offset;
  227. ts_encode_pts_dts(&data[9], 2, pts);
  228. }
  229. void ts_packet_change_pts_dts(uint8_t *ts_packet, uint64_t pts, uint64_t dts) {
  230. uint8_t payload_offset = ts_packet_get_payload_offset(ts_packet);
  231. if (!payload_offset)
  232. return;
  233. uint8_t *data = ts_packet + payload_offset;
  234. ts_encode_pts_dts(&data[9], 3, pts);
  235. ts_encode_pts_dts(&data[14], 1, dts);
  236. }
  237. int ts_packet_has_pcr(uint8_t *ts_packet) {
  238. if (ts_packet[0] == 0x47) { // TS packet
  239. if (bit_on(ts_packet[3], bit_6)) { // Packet have adaptation field
  240. if (ts_packet[4] > 6) { // Adaptation field length is > 6
  241. if (bit_on(ts_packet[5], bit_5)) { // The is PCR field
  242. return 1;
  243. } else {
  244. // ts_LOGf("!no PCR field\n");
  245. }
  246. } else {
  247. // ts_LOGf("!not enough adaptation len (%d), need at least 7\n", ts_packet[4]);
  248. }
  249. } else {
  250. // ts_LOGf("!no adaptation field\n");
  251. }
  252. } else {
  253. // ts_LOGf("!no ts packet start (0x%02x) need 0x47\n", ts_packet[0]);
  254. }
  255. return 0;
  256. }
  257. uint64_t ts_packet_get_pcr_ex(uint8_t *ts_packet, uint64_t *pcr_base, uint16_t *pcr_ext) {
  258. uint8_t *data = ts_packet + 6; // Offset in TS packet
  259. *pcr_base = (uint64_t)data[0] << 25;
  260. *pcr_base += (uint64_t)data[1] << 17;
  261. *pcr_base += (uint64_t)data[2] << 9;
  262. *pcr_base += (uint64_t)data[3] << 1;
  263. *pcr_base += (uint64_t)data[4] >> 7;
  264. *pcr_ext = ((uint16_t)data[4] & 0x01) << 8;
  265. *pcr_ext += (uint16_t)data[5];
  266. //ts_LOGf("get pcr_base=%10llu pcr_ext=%4u pcr=%llu\n", *pcr_base, *pcr_ext, *pcr_base * 300ll + *pcr_ext);
  267. return *pcr_base * 300ll + *pcr_ext;
  268. }
  269. uint64_t ts_packet_get_pcr(uint8_t *ts_packet) {
  270. uint64_t pcr_base;
  271. uint16_t pcr_ext;
  272. return ts_packet_get_pcr_ex(ts_packet, &pcr_base, &pcr_ext);
  273. }
  274. void ts_packet_set_pcr_ex(uint8_t *ts_packet, uint64_t pcr_base, uint16_t pcr_ext) {
  275. //ts_LOGf("set pcr_base=%10llu pcr_ext=%4u pcr=%llu\n", pcr_base, pcr_ext, pcr);
  276. // 6 is the PCR offset in ts_packet (4 bytes header, 1 byte adapt field len)
  277. ts_packet[6 + 0] = (pcr_base >> 25) & 0xFF;
  278. ts_packet[6 + 1] = (pcr_base >> 17) & 0xFF;
  279. ts_packet[6 + 2] = (pcr_base >> 9) & 0xFF;
  280. ts_packet[6 + 3] = (pcr_base >> 1) & 0xFF;
  281. ts_packet[6 + 4] = 0x7e | ((pcr_ext >> 8) & 0x01) | ((pcr_base & 0x01) <<7 ); // 0x7e == 6 reserved bits
  282. ts_packet[6 + 5] = pcr_ext & 0xFF;
  283. }
  284. void ts_packet_set_pcr(uint8_t *ts_packet, uint64_t pcr) {
  285. uint64_t pcr_base = pcr / 300;
  286. uint16_t pcr_ext = pcr % 300;
  287. ts_packet_set_pcr_ex(ts_packet, pcr_base, pcr_ext);
  288. }
  289. uint8_t *ts_packet_header_parse(uint8_t *ts_packet, struct ts_header *ts_header) {
  290. if (ts_packet[0] != 0x47) {
  291. // ts_LOGf("*** TS packet do not start with sync byte 0x47 but with 0x%02x\n", ts_packet[0]);
  292. goto return_error;
  293. }
  294. ts_header->tei = (ts_packet[1] &~ 0x7f) >> 7; // x1111111
  295. ts_header->pusi = (ts_packet[1] &~ 0xbf) >> 6; // 1x111111
  296. ts_header->prio = (ts_packet[1] &~ 0xdf) >> 5; // 11x11111
  297. ts_header->pid = (ts_packet[1] &~ 0xE0) << 8 | ts_packet[2]; // 111xxxxx xxxxxxxx
  298. ts_header->scramble = (ts_packet[3] &~ 0x3F) >> 6; // xx111111
  299. ts_header->adapt_field = (ts_packet[3] &~ 0xDF) >> 5; // 11x11111
  300. ts_header->payload_field = (ts_packet[3] &~ 0xEF) >> 4; // 111x1111
  301. ts_header->continuity = (ts_packet[3] &~ 0xF0); // 1111xxxx
  302. if (!ts_header->adapt_field) {
  303. ts_header->adapt_len = 0;
  304. ts_header->adapt_flags = 0;
  305. ts_header->payload_offset = 4;
  306. } else {
  307. ts_header->adapt_len = ts_packet[4];
  308. if (ts_header->adapt_len) {
  309. ts_header->adapt_flags = ts_packet[5];
  310. }
  311. ts_header->payload_offset = 5 + ts_header->adapt_len; // 2 bytes see above
  312. }
  313. if (!ts_header->adapt_field && !ts_header->payload_field) // Not allowed
  314. goto return_error;
  315. if (!ts_header->payload_field)
  316. return NULL;
  317. if (ts_header->payload_field && ts_header->adapt_len > 182) // Validity checks
  318. goto return_error;
  319. if (!ts_header->payload_field && ts_header->adapt_len > 183)
  320. goto return_error;
  321. if (ts_header->payload_offset > TS_MAX_PAYLOAD_SIZE) // Validity check
  322. goto return_error;
  323. ts_header->payload_size = TS_PACKET_SIZE - ts_header->payload_offset;
  324. return ts_packet + ts_header->payload_offset;
  325. return_error:
  326. memset(ts_header, 0, sizeof(struct ts_header));
  327. return NULL;
  328. }
  329. void ts_packet_header_generate(uint8_t *ts_packet, struct ts_header *ts_header) {
  330. memset(ts_packet, 0xFF, TS_PACKET_SIZE);
  331. ts_packet[0] = 0x47;
  332. ts_packet[1] = 0;
  333. ts_packet[1] = ts_header->tei << 7; // x1111111
  334. ts_packet[1] |= ts_header->pusi << 6; // 1x111111
  335. ts_packet[1] |= ts_header->prio << 5; // 11x11111
  336. ts_packet[1] |= ts_header->pid >> 8; // 111xxxxx xxxxxxxx
  337. ts_packet[2] = ts_header->pid &~ 0xff00;
  338. ts_packet[3] = 0;
  339. ts_packet[3] = ts_header->scramble << 6; // xx111111
  340. ts_packet[3] |= ts_header->adapt_field << 5; // 11x11111
  341. ts_packet[3] |= ts_header->payload_field << 4; // 111x1111
  342. ts_packet[3] |= ts_header->continuity; // 1111xxxx
  343. if (ts_header->adapt_field) {
  344. ts_packet[4] = ts_header->adapt_len;
  345. ts_packet[5] = ts_header->adapt_flags;
  346. }
  347. }
  348. void ts_packet_header_dump(struct ts_header *ts_header) {
  349. 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",
  350. ts_header->tei,
  351. ts_header->pusi,
  352. ts_header->prio,
  353. ts_header->pid,
  354. ts_header->pid,
  355. ts_header->scramble,
  356. ts_header->adapt_field,
  357. ts_header->payload_field,
  358. ts_header->adapt_len,
  359. ts_header->adapt_flags,
  360. ts_header->payload_offset,
  361. ts_header->payload_size
  362. );
  363. }