0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018 #define pr_fmt(fmt) KBUILD_MODNAME ":%s, %d: " fmt, __func__, __LINE__
0019
0020 #include <linux/bug.h>
0021 #include <linux/crc32.h>
0022 #include <linux/fixp-arith.h>
0023 #include <linux/jiffies.h>
0024 #include <linux/kernel.h>
0025 #include <linux/math64.h>
0026 #include <linux/printk.h>
0027 #include <linux/ratelimit.h>
0028 #include <linux/slab.h>
0029 #include <linux/string.h>
0030 #include <linux/types.h>
0031 #include <linux/vmalloc.h>
0032
0033 #include "vidtv_common.h"
0034 #include "vidtv_encoder.h"
0035 #include "vidtv_s302m.h"
0036
0037 #define S302M_SAMPLING_RATE_HZ 48000
0038 #define PES_PRIVATE_STREAM_1 0xbd
0039 #define S302M_BLOCK_SZ 192
0040 #define S302M_SIN_LUT_NUM_ELEM 1024
0041
0042
0043 #define FF_S302M_DEFAULT_NUM_FRAMES 1115
0044 #define FF_S302M_DEFAULT_PTS_INCREMENT 2090
0045 #define FF_S302M_DEFAULT_PTS_OFFSET 100000
0046
0047
0048 #define PI 180
0049
0050 static const u8 reverse[256] = {
0051
0052 0x00, 0x80, 0x40, 0xC0, 0x20, 0xA0, 0x60, 0xE0, 0x10, 0x90, 0x50, 0xD0,
0053 0x30, 0xB0, 0x70, 0xF0, 0x08, 0x88, 0x48, 0xC8, 0x28, 0xA8, 0x68, 0xE8,
0054 0x18, 0x98, 0x58, 0xD8, 0x38, 0xB8, 0x78, 0xF8, 0x04, 0x84, 0x44, 0xC4,
0055 0x24, 0xA4, 0x64, 0xE4, 0x14, 0x94, 0x54, 0xD4, 0x34, 0xB4, 0x74, 0xF4,
0056 0x0C, 0x8C, 0x4C, 0xCC, 0x2C, 0xAC, 0x6C, 0xEC, 0x1C, 0x9C, 0x5C, 0xDC,
0057 0x3C, 0xBC, 0x7C, 0xFC, 0x02, 0x82, 0x42, 0xC2, 0x22, 0xA2, 0x62, 0xE2,
0058 0x12, 0x92, 0x52, 0xD2, 0x32, 0xB2, 0x72, 0xF2, 0x0A, 0x8A, 0x4A, 0xCA,
0059 0x2A, 0xAA, 0x6A, 0xEA, 0x1A, 0x9A, 0x5A, 0xDA, 0x3A, 0xBA, 0x7A, 0xFA,
0060 0x06, 0x86, 0x46, 0xC6, 0x26, 0xA6, 0x66, 0xE6, 0x16, 0x96, 0x56, 0xD6,
0061 0x36, 0xB6, 0x76, 0xF6, 0x0E, 0x8E, 0x4E, 0xCE, 0x2E, 0xAE, 0x6E, 0xEE,
0062 0x1E, 0x9E, 0x5E, 0xDE, 0x3E, 0xBE, 0x7E, 0xFE, 0x01, 0x81, 0x41, 0xC1,
0063 0x21, 0xA1, 0x61, 0xE1, 0x11, 0x91, 0x51, 0xD1, 0x31, 0xB1, 0x71, 0xF1,
0064 0x09, 0x89, 0x49, 0xC9, 0x29, 0xA9, 0x69, 0xE9, 0x19, 0x99, 0x59, 0xD9,
0065 0x39, 0xB9, 0x79, 0xF9, 0x05, 0x85, 0x45, 0xC5, 0x25, 0xA5, 0x65, 0xE5,
0066 0x15, 0x95, 0x55, 0xD5, 0x35, 0xB5, 0x75, 0xF5, 0x0D, 0x8D, 0x4D, 0xCD,
0067 0x2D, 0xAD, 0x6D, 0xED, 0x1D, 0x9D, 0x5D, 0xDD, 0x3D, 0xBD, 0x7D, 0xFD,
0068 0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3, 0x13, 0x93, 0x53, 0xD3,
0069 0x33, 0xB3, 0x73, 0xF3, 0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB,
0070 0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB, 0x07, 0x87, 0x47, 0xC7,
0071 0x27, 0xA7, 0x67, 0xE7, 0x17, 0x97, 0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7,
0072 0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF, 0x1F, 0x9F, 0x5F, 0xDF,
0073 0x3F, 0xBF, 0x7F, 0xFF,
0074 };
0075
0076 struct tone_duration {
0077 enum musical_notes note;
0078 int duration;
0079 };
0080
0081 #define COMPASS 100
0082 static const struct tone_duration beethoven_fur_elise[] = {
0083 { NOTE_SILENT, 512},
0084 { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
0085 { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
0086 { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
0087 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
0088 { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
0089 { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
0090 { NOTE_GS_5, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
0091 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_E_5, 128},
0092 { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
0093 { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
0094 { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
0095 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
0096 { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
0097 { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
0098 { NOTE_C_6, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
0099 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_SILENT, 128},
0100
0101 { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
0102 { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
0103 { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
0104 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
0105 { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
0106 { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
0107 { NOTE_GS_5, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
0108 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_E_5, 128},
0109 { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
0110 { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
0111 { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
0112 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
0113 { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
0114 { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
0115 { NOTE_C_6, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
0116 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_B_4, 128},
0117 { NOTE_C_5, 128}, { NOTE_D_5, 128}, { NOTE_C_4, 128},
0118 { NOTE_G_4, 128}, { NOTE_C_5, 128}, { NOTE_G_4, 128},
0119 { NOTE_F_5, 128}, { NOTE_E_5, 128}, { NOTE_G_3, 128},
0120 { NOTE_G_4, 128}, { NOTE_B_3, 128}, { NOTE_F_4, 128},
0121 { NOTE_E_5, 128}, { NOTE_D_5, 128}, { NOTE_A_3, 128},
0122 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_E_4, 128},
0123 { NOTE_D_5, 128}, { NOTE_C_5, 128}, { NOTE_E_3, 128},
0124 { NOTE_E_4, 128}, { NOTE_E_5, 128}, { NOTE_E_5, 128},
0125 { NOTE_E_6, 128}, { NOTE_E_5, 128}, { NOTE_E_6, 128},
0126 { NOTE_E_5, 128}, { NOTE_E_5, 128}, { NOTE_DS_5, 128},
0127 { NOTE_E_5, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
0128 { NOTE_DS_5, 128}, { NOTE_E_5, 128}, { NOTE_DS_6, 128},
0129 { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
0130 { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
0131 { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
0132 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
0133 { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
0134 { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
0135 { NOTE_GS_5, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
0136 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_E_5, 128},
0137 { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
0138 { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
0139 { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
0140 { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
0141 { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
0142 { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
0143 { NOTE_C_6, 128}, { NOTE_B_5, 128}, { NOTE_A_5, 512},
0144 { NOTE_SILENT, 256},
0145 };
0146
0147 static struct vidtv_access_unit *vidtv_s302m_access_unit_init(struct vidtv_access_unit *head)
0148 {
0149 struct vidtv_access_unit *au;
0150
0151 au = kzalloc(sizeof(*au), GFP_KERNEL);
0152 if (!au)
0153 return NULL;
0154
0155 if (head) {
0156 while (head->next)
0157 head = head->next;
0158
0159 head->next = au;
0160 }
0161
0162 return au;
0163 }
0164
0165 static void vidtv_s302m_access_unit_destroy(struct vidtv_encoder *e)
0166 {
0167 struct vidtv_access_unit *head = e->access_units;
0168 struct vidtv_access_unit *tmp = NULL;
0169
0170 while (head) {
0171 tmp = head;
0172 head = head->next;
0173 kfree(tmp);
0174 }
0175
0176 e->access_units = NULL;
0177 }
0178
0179 static void vidtv_s302m_alloc_au(struct vidtv_encoder *e)
0180 {
0181 struct vidtv_access_unit *sync_au = NULL;
0182 struct vidtv_access_unit *temp = NULL;
0183
0184 if (e->sync && e->sync->is_video_encoder) {
0185 sync_au = e->sync->access_units;
0186
0187 while (sync_au) {
0188 temp = vidtv_s302m_access_unit_init(e->access_units);
0189 if (!e->access_units)
0190 e->access_units = temp;
0191
0192 sync_au = sync_au->next;
0193 }
0194
0195 return;
0196 }
0197
0198 e->access_units = vidtv_s302m_access_unit_init(NULL);
0199 }
0200
0201 static void
0202 vidtv_s302m_compute_sample_count_from_video(struct vidtv_encoder *e)
0203 {
0204 struct vidtv_access_unit *sync_au = e->sync->access_units;
0205 struct vidtv_access_unit *au = e->access_units;
0206 u32 sample_duration_usecs;
0207 u32 vau_duration_usecs;
0208 u32 s;
0209
0210 vau_duration_usecs = USEC_PER_SEC / e->sync->sampling_rate_hz;
0211 sample_duration_usecs = USEC_PER_SEC / e->sampling_rate_hz;
0212
0213 while (au && sync_au) {
0214 s = DIV_ROUND_UP(vau_duration_usecs, sample_duration_usecs);
0215 au->num_samples = s;
0216 au = au->next;
0217 sync_au = sync_au->next;
0218 }
0219 }
0220
0221 static void vidtv_s302m_compute_pts_from_video(struct vidtv_encoder *e)
0222 {
0223 struct vidtv_access_unit *au = e->access_units;
0224 struct vidtv_access_unit *sync_au = e->sync->access_units;
0225
0226
0227 while (au && sync_au) {
0228 au->pts = sync_au->pts;
0229 au = au->next;
0230 sync_au = sync_au->next;
0231 }
0232 }
0233
0234 static u16 vidtv_s302m_get_sample(struct vidtv_encoder *e)
0235 {
0236 u16 sample;
0237 int pos;
0238 struct vidtv_s302m_ctx *ctx = e->ctx;
0239
0240 if (!e->src_buf) {
0241
0242
0243
0244
0245 if (ctx->last_duration <= 0) {
0246 if (e->src_buf_offset >= ARRAY_SIZE(beethoven_fur_elise))
0247 e->src_buf_offset = 0;
0248
0249 ctx->last_tone = beethoven_fur_elise[e->src_buf_offset].note;
0250 ctx->last_duration = beethoven_fur_elise[e->src_buf_offset].duration *
0251 S302M_SAMPLING_RATE_HZ / COMPASS / 5;
0252 e->src_buf_offset++;
0253 ctx->note_offset = 0;
0254 } else {
0255 ctx->last_duration--;
0256 }
0257
0258
0259 if (!ctx->last_tone)
0260 return 0x8000;
0261
0262 pos = (2 * PI * ctx->note_offset * ctx->last_tone) / S302M_SAMPLING_RATE_HZ;
0263 ctx->note_offset++;
0264
0265 return (fixp_sin32(pos % (2 * PI)) >> 16) + 0x8000;
0266 }
0267
0268
0269 if (e->src_buf_offset > e->src_buf_sz) {
0270 pr_err_ratelimited("overflow detected: %d > %d, wrapping.\n",
0271 e->src_buf_offset,
0272 e->src_buf_sz);
0273
0274 e->src_buf_offset = 0;
0275 }
0276
0277 if (e->src_buf_offset >= e->src_buf_sz) {
0278
0279 if (e->last_sample_cb)
0280 e->last_sample_cb(e->sample_count);
0281
0282 e->src_buf_offset = 0;
0283 }
0284
0285 sample = *(u16 *)(e->src_buf + e->src_buf_offset);
0286
0287 return sample;
0288 }
0289
0290 static u32 vidtv_s302m_write_frame(struct vidtv_encoder *e,
0291 u16 sample)
0292 {
0293 struct vidtv_s302m_ctx *ctx = e->ctx;
0294 struct vidtv_s302m_frame_16 f = {};
0295 u32 nbytes = 0;
0296
0297
0298
0299 u8 vucf = ctx->frame_index == 0 ? 0x10 : 0;
0300
0301 f.data[0] = sample & 0xFF;
0302 f.data[1] = (sample & 0xFF00) >> 8;
0303 f.data[2] = ((sample & 0x0F) << 4) | vucf;
0304 f.data[3] = (sample & 0x0FF0) >> 4;
0305 f.data[4] = (sample & 0xF000) >> 12;
0306
0307 f.data[0] = reverse[f.data[0]];
0308 f.data[1] = reverse[f.data[1]];
0309 f.data[2] = reverse[f.data[2]];
0310 f.data[3] = reverse[f.data[3]];
0311 f.data[4] = reverse[f.data[4]];
0312
0313 nbytes += vidtv_memcpy(e->encoder_buf,
0314 e->encoder_buf_offset,
0315 VIDTV_S302M_BUF_SZ,
0316 &f,
0317 sizeof(f));
0318
0319 e->encoder_buf_offset += nbytes;
0320
0321 ctx->frame_index++;
0322 if (ctx->frame_index >= S302M_BLOCK_SZ)
0323 ctx->frame_index = 0;
0324
0325 return nbytes;
0326 }
0327
0328 static u32 vidtv_s302m_write_h(struct vidtv_encoder *e, u32 p_sz)
0329 {
0330 struct vidtv_smpte_s302m_es h = {};
0331 u32 nbytes = 0;
0332
0333
0334 h.bitfield = cpu_to_be32((p_sz << 16));
0335
0336 nbytes += vidtv_memcpy(e->encoder_buf,
0337 e->encoder_buf_offset,
0338 e->encoder_buf_sz,
0339 &h,
0340 sizeof(h));
0341
0342 e->encoder_buf_offset += nbytes;
0343 return nbytes;
0344 }
0345
0346 static void vidtv_s302m_write_frames(struct vidtv_encoder *e)
0347 {
0348 struct vidtv_access_unit *au = e->access_units;
0349 struct vidtv_s302m_ctx *ctx = e->ctx;
0350 u32 nbytes_per_unit = 0;
0351 u32 nbytes = 0;
0352 u32 au_sz = 0;
0353 u16 sample;
0354 u32 j;
0355
0356 while (au) {
0357 au_sz = au->num_samples *
0358 sizeof(struct vidtv_s302m_frame_16);
0359
0360 nbytes_per_unit = vidtv_s302m_write_h(e, au_sz);
0361
0362 for (j = 0; j < au->num_samples; ++j) {
0363 sample = vidtv_s302m_get_sample(e);
0364 nbytes_per_unit += vidtv_s302m_write_frame(e, sample);
0365
0366 if (e->src_buf)
0367 e->src_buf_offset += sizeof(u16);
0368
0369 e->sample_count++;
0370 }
0371
0372 au->nbytes = nbytes_per_unit;
0373
0374 if (au_sz + sizeof(struct vidtv_smpte_s302m_es) != nbytes_per_unit) {
0375 pr_warn_ratelimited("write size was %u, expected %zu\n",
0376 nbytes_per_unit,
0377 au_sz + sizeof(struct vidtv_smpte_s302m_es));
0378 }
0379
0380 nbytes += nbytes_per_unit;
0381 au->offset = nbytes - nbytes_per_unit;
0382
0383 nbytes_per_unit = 0;
0384 ctx->au_count++;
0385
0386 au = au->next;
0387 }
0388 }
0389
0390 static void *vidtv_s302m_encode(struct vidtv_encoder *e)
0391 {
0392 struct vidtv_s302m_ctx *ctx = e->ctx;
0393
0394
0395
0396
0397
0398
0399
0400
0401
0402
0403
0404
0405
0406
0407 vidtv_s302m_access_unit_destroy(e);
0408 vidtv_s302m_alloc_au(e);
0409
0410 if (e->sync && e->sync->is_video_encoder) {
0411 vidtv_s302m_compute_sample_count_from_video(e);
0412 vidtv_s302m_compute_pts_from_video(e);
0413 } else {
0414 e->access_units->num_samples = FF_S302M_DEFAULT_NUM_FRAMES;
0415 e->access_units->pts = (ctx->au_count * FF_S302M_DEFAULT_PTS_INCREMENT) +
0416 FF_S302M_DEFAULT_PTS_OFFSET;
0417 }
0418
0419 vidtv_s302m_write_frames(e);
0420
0421 return e->encoder_buf;
0422 }
0423
0424 static u32 vidtv_s302m_clear(struct vidtv_encoder *e)
0425 {
0426 struct vidtv_access_unit *au = e->access_units;
0427 u32 count = 0;
0428
0429 while (au) {
0430 count++;
0431 au = au->next;
0432 }
0433
0434 vidtv_s302m_access_unit_destroy(e);
0435 memset(e->encoder_buf, 0, VIDTV_S302M_BUF_SZ);
0436 e->encoder_buf_offset = 0;
0437
0438 return count;
0439 }
0440
0441 struct vidtv_encoder
0442 *vidtv_s302m_encoder_init(struct vidtv_s302m_encoder_init_args args)
0443 {
0444 u32 priv_sz = sizeof(struct vidtv_s302m_ctx);
0445 struct vidtv_s302m_ctx *ctx;
0446 struct vidtv_encoder *e;
0447
0448 e = kzalloc(sizeof(*e), GFP_KERNEL);
0449 if (!e)
0450 return NULL;
0451
0452 e->id = S302M;
0453
0454 if (args.name)
0455 e->name = kstrdup(args.name, GFP_KERNEL);
0456
0457 e->encoder_buf = vzalloc(VIDTV_S302M_BUF_SZ);
0458 if (!e->encoder_buf)
0459 goto out_kfree_e;
0460
0461 e->encoder_buf_sz = VIDTV_S302M_BUF_SZ;
0462 e->encoder_buf_offset = 0;
0463
0464 e->sample_count = 0;
0465
0466 e->src_buf = (args.src_buf) ? args.src_buf : NULL;
0467 e->src_buf_sz = (args.src_buf) ? args.src_buf_sz : 0;
0468 e->src_buf_offset = 0;
0469
0470 e->is_video_encoder = false;
0471
0472 ctx = kzalloc(priv_sz, GFP_KERNEL);
0473 if (!ctx)
0474 goto out_kfree_buf;
0475
0476 e->ctx = ctx;
0477 ctx->last_duration = 0;
0478
0479 e->encode = vidtv_s302m_encode;
0480 e->clear = vidtv_s302m_clear;
0481
0482 e->es_pid = cpu_to_be16(args.es_pid);
0483 e->stream_id = cpu_to_be16(PES_PRIVATE_STREAM_1);
0484
0485 e->sync = args.sync;
0486 e->sampling_rate_hz = S302M_SAMPLING_RATE_HZ;
0487
0488 e->last_sample_cb = args.last_sample_cb;
0489
0490 e->destroy = vidtv_s302m_encoder_destroy;
0491
0492 if (args.head) {
0493 while (args.head->next)
0494 args.head = args.head->next;
0495
0496 args.head->next = e;
0497 }
0498
0499 e->next = NULL;
0500
0501 return e;
0502
0503 out_kfree_buf:
0504 vfree(e->encoder_buf);
0505
0506 out_kfree_e:
0507 kfree(e->name);
0508 kfree(e);
0509 return NULL;
0510 }
0511
0512 void vidtv_s302m_encoder_destroy(struct vidtv_encoder *e)
0513 {
0514 if (e->id != S302M) {
0515 pr_err_ratelimited("Encoder type mismatch, skipping.\n");
0516 return;
0517 }
0518
0519 vidtv_s302m_access_unit_destroy(e);
0520 kfree(e->name);
0521 vfree(e->encoder_buf);
0522 kfree(e->ctx);
0523 kfree(e);
0524 }