rtime.felk.cvut.cz Git - can-eth-gw-linux.git/blob

1 // SPDX-License-Identifier: GPL-2.0

2 /*

3 * Vidtv serves as a reference DVB driver and helps validate the existing APIs

4 * in the media subsystem. It can also aid developers working on userspace

5 * applications.

6 *

7 * This file contains the multiplexer logic for TS packets from different

8 * elementary streams

9 *

10 * Loosely based on libavcodec/mpegtsenc.c

11 *

13 */

15 #include <linux/delay.h>

16 #include <linux/dev_printk.h>

17 #include <linux/jiffies.h>

18 #include <linux/kernel.h>

19 #include <linux/math64.h>

20 #include <linux/ratelimit.h>

21 #include <linux/slab.h>

22 #include <linux/types.h>

23 #include <linux/vmalloc.h>

25 #include "vidtv_channel.h"

26 #include "vidtv_common.h"

27 #include "vidtv_encoder.h"

28 #include "vidtv_mux.h"

29 #include "vidtv_pes.h"

30 #include "vidtv_psi.h"

31 #include "vidtv_ts.h"

33 static struct vidtv_mux_pid_ctx

34 *vidtv_mux_get_pid_ctx(struct vidtv_mux *m, u16 pid)

35 {

36 struct vidtv_mux_pid_ctx *ctx;

38 hash_for_each_possible(m->pid_ctx, ctx, h, pid)

39 if (ctx->pid == pid)

40 return ctx;

41 return NULL;

42 }

44 static struct vidtv_mux_pid_ctx

45 *vidtv_mux_create_pid_ctx_once(struct vidtv_mux *m, u16 pid)

46 {

47 struct vidtv_mux_pid_ctx *ctx;

49 ctx = vidtv_mux_get_pid_ctx(m, pid);

50 if (ctx)

51 return ctx;

53 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);

54 if (!ctx)

55 return NULL;

57 ctx->pid = pid;

58 ctx->cc = 0;

59 hash_add(m->pid_ctx, &ctx->h, pid);

61 return ctx;

62 }

64 static void vidtv_mux_pid_ctx_destroy(struct vidtv_mux *m)

65 {

66 struct vidtv_mux_pid_ctx *ctx;

67 struct hlist_node *tmp;

68 int bkt;

70 hash_for_each_safe(m->pid_ctx, bkt, tmp, ctx, h) {

71 hash_del(&ctx->h);

72 kfree(ctx);

73 }

74 }

76 static int vidtv_mux_pid_ctx_init(struct vidtv_mux *m)

77 {

78 struct vidtv_psi_table_pat_program *p = m->si.pat->program;

79 u16 pid;

81 hash_init(m->pid_ctx);

82 /* push the pcr pid ctx */

83 if (!vidtv_mux_create_pid_ctx_once(m, m->pcr_pid))

84 return -ENOMEM;

85 /* push the NULL packet pid ctx */

86 if (!vidtv_mux_create_pid_ctx_once(m, TS_NULL_PACKET_PID))

87 goto free;

88 /* push the PAT pid ctx */

89 if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_PAT_PID))

90 goto free;

91 /* push the SDT pid ctx */

92 if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_SDT_PID))

93 goto free;

94 /* push the NIT pid ctx */

95 if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_NIT_PID))

96 goto free;

97 /* push the EIT pid ctx */

98 if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_EIT_PID))

99 goto free;

100

101 /* add a ctx for all PMT sections */

102 while (p) {

103 pid = vidtv_psi_get_pat_program_pid(p);

104 vidtv_mux_create_pid_ctx_once(m, pid);

105 p = p->next;

106 }

107

108 return 0;

109

110 free:

111 vidtv_mux_pid_ctx_destroy(m);

112 return -ENOMEM;

113 }

114

115 static void vidtv_mux_update_clk(struct vidtv_mux *m)

116 {

117 /* call this at every thread iteration */

118 u64 elapsed_time;

119

120 m->timing.past_jiffies = m->timing.current_jiffies;

121 m->timing.current_jiffies = get_jiffies_64();

122

123 elapsed_time = jiffies_to_usecs(m->timing.current_jiffies -

124 m->timing.past_jiffies);

125

126 /* update the 27Mhz clock proportionally to the elapsed time */

127 m->timing.clk += (CLOCK_UNIT_27MHZ / USEC_PER_SEC) * elapsed_time;

128 }

129

130 static u32 vidtv_mux_push_si(struct vidtv_mux *m)

131 {

132 struct vidtv_psi_pat_write_args pat_args = {

133 .buf = m->mux_buf,

134 .buf_sz = m->mux_buf_sz,

135 .pat = m->si.pat,

136 };

137 struct vidtv_psi_pmt_write_args pmt_args = {

138 .buf = m->mux_buf,

139 .buf_sz = m->mux_buf_sz,

140 .pcr_pid = m->pcr_pid,

141 };

142 struct vidtv_psi_sdt_write_args sdt_args = {

143 .buf = m->mux_buf,

144 .buf_sz = m->mux_buf_sz,

145 .sdt = m->si.sdt,

146 };

147 struct vidtv_psi_nit_write_args nit_args = {

148 .buf = m->mux_buf,

149 .buf_sz = m->mux_buf_sz,

150 .nit = m->si.nit,

151

152 };

153 struct vidtv_psi_eit_write_args eit_args = {

154 .buf = m->mux_buf,

155 .buf_sz = m->mux_buf_sz,

156 .eit = m->si.eit,

157 };

158 u32 initial_offset = m->mux_buf_offset;

159 struct vidtv_mux_pid_ctx *pat_ctx;

160 struct vidtv_mux_pid_ctx *pmt_ctx;

161 struct vidtv_mux_pid_ctx *sdt_ctx;

162 struct vidtv_mux_pid_ctx *nit_ctx;

163 struct vidtv_mux_pid_ctx *eit_ctx;

164 u32 nbytes;

165 u16 pmt_pid;

166 u32 i;

167

168 pat_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_PAT_PID);

169 sdt_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_SDT_PID);

170 nit_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_NIT_PID);

171 eit_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_EIT_PID);

172

173 pat_args.offset = m->mux_buf_offset;

174 pat_args.continuity_counter = &pat_ctx->cc;

175

176 m->mux_buf_offset += vidtv_psi_pat_write_into(&pat_args);

177

178 for (i = 0; i < m->si.pat->num_pmt; ++i) {

179 pmt_pid = vidtv_psi_pmt_get_pid(m->si.pmt_secs[i],

180 m->si.pat);

181

182 if (pmt_pid > TS_LAST_VALID_PID) {

183 dev_warn_ratelimited(m->dev,

184 "PID: %d not found\n", pmt_pid);

185 continue;

186 }

187

188 pmt_ctx = vidtv_mux_get_pid_ctx(m, pmt_pid);

189

190 pmt_args.offset = m->mux_buf_offset;

191 pmt_args.pmt = m->si.pmt_secs[i];

192 pmt_args.pid = pmt_pid;

193 pmt_args.continuity_counter = &pmt_ctx->cc;

194

195 /* write each section into buffer */

196 m->mux_buf_offset += vidtv_psi_pmt_write_into(pmt_args);

197 }

198

199 sdt_args.offset = m->mux_buf_offset;

200 sdt_args.continuity_counter = &sdt_ctx->cc;

201

202 m->mux_buf_offset += vidtv_psi_sdt_write_into(sdt_args);

203

204 nit_args.offset = m->mux_buf_offset;

205 nit_args.continuity_counter = &nit_ctx->cc;

206

207 m->mux_buf_offset += vidtv_psi_nit_write_into(nit_args);

208

209 eit_args.offset = m->mux_buf_offset;

210 eit_args.continuity_counter = &eit_ctx->cc;

211

212 m->mux_buf_offset += vidtv_psi_eit_write_into(eit_args);

213

214 nbytes = m->mux_buf_offset - initial_offset;

215

216 m->num_streamed_si++;

217

218 return nbytes;

219 }

220

221 static u32 vidtv_mux_push_pcr(struct vidtv_mux *m)

222 {

223 struct pcr_write_args args = {};

224 struct vidtv_mux_pid_ctx *ctx;

225 u32 nbytes = 0;

226

227 ctx = vidtv_mux_get_pid_ctx(m, m->pcr_pid);

228 args.dest_buf = m->mux_buf;

229 args.pid = m->pcr_pid;

230 args.buf_sz = m->mux_buf_sz;

231 args.continuity_counter = &ctx->cc;

232

233 /* the 27Mhz clock will feed both parts of the PCR bitfield */

234 args.pcr = m->timing.clk;

235

236 nbytes += vidtv_ts_pcr_write_into(args);

237 m->mux_buf_offset += nbytes;

238

239 m->num_streamed_pcr++;

240

241 return nbytes;

242 }

243

244 static bool vidtv_mux_should_push_pcr(struct vidtv_mux *m)

245 {

246 u64 next_pcr_at;

247

248 if (m->num_streamed_pcr == 0)

249 return true;

250

251 next_pcr_at = m->timing.start_jiffies +

252 usecs_to_jiffies(m->num_streamed_pcr *

253 m->timing.pcr_period_usecs);

254

255 return time_after64(m->timing.current_jiffies, next_pcr_at);

256 }

257

258 static bool vidtv_mux_should_push_si(struct vidtv_mux *m)

259 {

260 u64 next_si_at;

261

262 if (m->num_streamed_si == 0)

263 return true;

264

265 next_si_at = m->timing.start_jiffies +

266 usecs_to_jiffies(m->num_streamed_si *

267 m->timing.si_period_usecs);

268

269 return time_after64(m->timing.current_jiffies, next_si_at);

270 }

271

272 static u32 vidtv_mux_packetize_access_units(struct vidtv_mux *m,

273 struct vidtv_encoder *e)

274 {

275 struct pes_write_args args = {

276 .dest_buf = m->mux_buf,

277 .dest_buf_sz = m->mux_buf_sz,

278 .pid = be16_to_cpu(e->es_pid),

279 .encoder_id = e->id,

280 .stream_id = be16_to_cpu(e->stream_id),

281 .send_pts = true, /* forbidden value '01'... */

282 .send_dts = false, /* ...for PTS_DTS flags */

283 };

284 struct vidtv_access_unit *au = e->access_units;

285 u32 initial_offset = m->mux_buf_offset;

286 struct vidtv_mux_pid_ctx *pid_ctx;

287 u32 nbytes = 0;

288 u8 *buf = NULL;

289

290 /* see SMPTE 302M clause 6.4 */

291 if (args.encoder_id == S302M) {

292 args.send_dts = false;

293 args.send_pts = true;

294 }

295

296 pid_ctx = vidtv_mux_create_pid_ctx_once(m, be16_to_cpu(e->es_pid));

297 args.continuity_counter = &pid_ctx->cc;

298

299 while (au) {

300 buf = e->encoder_buf + au->offset;

301 args.from = buf;

302 args.access_unit_len = au->nbytes;

303 args.dest_offset = m->mux_buf_offset;

304 args.pts = au->pts;

305 args.pcr = m->timing.clk;

306

307 m->mux_buf_offset += vidtv_pes_write_into(&args);

308

309 au = au->next;

310 }

311

312 /*

313 * clear the encoder state once the ES data has been written to the mux

314 * buffer

315 */

316 e->clear(e);

317

318 nbytes = m->mux_buf_offset - initial_offset;

319 return nbytes;

320 }

321

322 static u32 vidtv_mux_poll_encoders(struct vidtv_mux *m)

323 {

324 struct vidtv_channel *cur_chnl = m->channels;

325 struct vidtv_encoder *e = NULL;

326 u32 nbytes = 0;

327 u32 au_nbytes;

328

329 while (cur_chnl) {

330 e = cur_chnl->encoders;

331

332 while (e) {

333 e->encode(e);

334 /* get the TS packets into the mux buffer */

335 au_nbytes = vidtv_mux_packetize_access_units(m, e);

336 nbytes += au_nbytes;

337 m->mux_buf_offset += au_nbytes;

338 /* grab next encoder */

339 e = e->next;

340 }

341

342 /* grab the next channel */

343 cur_chnl = cur_chnl->next;

344 }

345

346 return nbytes;

347 }

348

349 static u32 vidtv_mux_pad_with_nulls(struct vidtv_mux *m, u32 npkts)

350 {

351 struct null_packet_write_args args = {

352 .dest_buf = m->mux_buf,

353 .buf_sz = m->mux_buf_sz,

354 .dest_offset = m->mux_buf_offset,

355 };

356 u32 initial_offset = m->mux_buf_offset;

357 struct vidtv_mux_pid_ctx *ctx;

358 u32 nbytes;

359 u32 i;

360

361 ctx = vidtv_mux_get_pid_ctx(m, TS_NULL_PACKET_PID);

362

363 args.continuity_counter = &ctx->cc;

364

365 for (i = 0; i < npkts; ++i) {

366 m->mux_buf_offset += vidtv_ts_null_write_into(args);

367 args.dest_offset = m->mux_buf_offset;

368 }

369

370 nbytes = m->mux_buf_offset - initial_offset;

371

372 /* sanity check */

373 if (nbytes != npkts * TS_PACKET_LEN)

374 dev_err_ratelimited(m->dev, "%d != %d\n",

375 nbytes, npkts * TS_PACKET_LEN);

376

377 return nbytes;

378 }

379

380 static void vidtv_mux_clear(struct vidtv_mux *m)

381 {

382 /* clear the packets currently in the mux */

383 memset(m->mux_buf, 0, m->mux_buf_sz * sizeof(*m->mux_buf));

384 /* point to the beginning of the buffer again */

385 m->mux_buf_offset = 0;

386 }

387

388 #define ERR_RATE 10000000

389 static void vidtv_mux_tick(struct work_struct *work)

390 {

391 struct vidtv_mux *m = container_of(work,

392 struct vidtv_mux,

393 mpeg_thread);

394 struct dtv_frontend_properties *c = &m->fe->dtv_property_cache;

395 u32 tot_bits = 0;

396 u32 nbytes;

397 u32 npkts;

398

399 while (m->streaming) {

400 nbytes = 0;

401

402 vidtv_mux_update_clk(m);

403

404 if (vidtv_mux_should_push_pcr(m))

405 nbytes += vidtv_mux_push_pcr(m);

406

407 if (vidtv_mux_should_push_si(m))

408 nbytes += vidtv_mux_push_si(m);

409

410 nbytes += vidtv_mux_poll_encoders(m);

411 nbytes += vidtv_mux_pad_with_nulls(m, 256);

412

413 npkts = nbytes / TS_PACKET_LEN;

414

415 /* if the buffer is not aligned there is a bug somewhere */

416 if (nbytes % TS_PACKET_LEN)

417 dev_err_ratelimited(m->dev, "Misaligned buffer\n");

418

419 if (m->on_new_packets_available_cb)

420 m->on_new_packets_available_cb(m->priv,

421 m->mux_buf,

422 npkts);

423

424 vidtv_mux_clear(m);

425

426 /*

427 * Update bytes and packet counts at DVBv5 stats

428 *

429 * For now, both pre and post bit counts are identical,

430 * but post BER count can be lower than pre BER, if the error

431 * correction logic discards packages.

432 */

433 c->pre_bit_count.stat[0].uvalue = nbytes * 8;

434 c->post_bit_count.stat[0].uvalue = nbytes * 8;

435 c->block_count.stat[0].uvalue += npkts;

436

437 /*

438 * Even without any visible errors for the user, the pre-BER

439 * stats usually have an error range up to 1E-6. So,

440 * add some random error increment count to it.

441 *

442 * Please notice that this is a poor guy's implementation,

443 * as it will produce one corrected bit error every time

444 * ceil(total bytes / ERR_RATE) is incremented, without

445 * any sort of (pseudo-)randomness.

446 */

447 tot_bits += nbytes * 8;

448 if (tot_bits > ERR_RATE) {

449 c->pre_bit_error.stat[0].uvalue++;

450 tot_bits -= ERR_RATE;

451 }

452

453 usleep_range(VIDTV_SLEEP_USECS, VIDTV_MAX_SLEEP_USECS);

454 }

455 }

456

457 void vidtv_mux_start_thread(struct vidtv_mux *m)

458 {

459 if (m->streaming) {

460 dev_warn_ratelimited(m->dev, "Already streaming. Skipping.\n");

461 return;

462 }

463

464 m->streaming = true;

465 m->timing.start_jiffies = get_jiffies_64();

466 schedule_work(&m->mpeg_thread);

467 }

468

469 void vidtv_mux_stop_thread(struct vidtv_mux *m)

470 {

471 if (m->streaming) {

472 m->streaming = false; /* thread will quit */

473 cancel_work_sync(&m->mpeg_thread);

474 }

475 }

476

477 struct vidtv_mux *vidtv_mux_init(struct dvb_frontend *fe,

478 struct device *dev,

479 struct vidtv_mux_init_args *args)

480 {

481 struct vidtv_mux *m;

482

483 m = kzalloc(sizeof(*m), GFP_KERNEL);

484 if (!m)

485 return NULL;

486

487 m->dev = dev;

488 m->fe = fe;

489 m->timing.pcr_period_usecs = args->pcr_period_usecs;

490 m->timing.si_period_usecs = args->si_period_usecs;

491

492 m->mux_rate_kbytes_sec = args->mux_rate_kbytes_sec;

493

494 m->on_new_packets_available_cb = args->on_new_packets_available_cb;

495

496 m->mux_buf = vzalloc(args->mux_buf_sz);

497 if (!m->mux_buf)

498 goto free_mux;

499

500 m->mux_buf_sz = args->mux_buf_sz;

501

502 m->pcr_pid = args->pcr_pid;

503 m->transport_stream_id = args->transport_stream_id;

504 m->priv = args->priv;

505 m->network_id = args->network_id;

506 m->network_name = kstrdup(args->network_name, GFP_KERNEL);

507 m->timing.current_jiffies = get_jiffies_64();

508

509 if (args->channels)

510 m->channels = args->channels;

511 else

512 if (vidtv_channels_init(m) < 0)

513 goto free_mux_buf;

514

515 /* will alloc data for pmt_sections after initializing pat */

516 if (vidtv_channel_si_init(m) < 0)

517 goto free_channels;

518

519 INIT_WORK(&m->mpeg_thread, vidtv_mux_tick);

520

521 if (vidtv_mux_pid_ctx_init(m) < 0)

522 goto free_channel_si;

523

524 return m;

525

526 free_channel_si:

527 vidtv_channel_si_destroy(m);

528 free_channels:

529 vidtv_channels_destroy(m);

530 free_mux_buf:

531 vfree(m->mux_buf);

532 free_mux:

533 kfree(m);

534 return NULL;

535 }

536

537 void vidtv_mux_destroy(struct vidtv_mux *m)

538 {

539 vidtv_mux_stop_thread(m);

540 vidtv_mux_pid_ctx_destroy(m);

541 vidtv_channel_si_destroy(m);

542 vidtv_channels_destroy(m);

543 kfree(m->network_name);

544 vfree(m->mux_buf);

545 kfree(m);

546 }