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

tsfuncs.c 9.5KB

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