776 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Copyright (C) 2007, 2008 Karsten Wiese <fzu@wemgehoertderstaat.de>4 */5 6#include <linux/usb.h>7#include <linux/gfp.h>8 9#include "usb_stream.h"10 11/* setup */12 13static unsigned int usb_stream_next_packet_size(struct usb_stream_kernel *sk)14{15 struct usb_stream *s = sk->s;16 17 sk->out_phase_peeked = (sk->out_phase & 0xffff) + sk->freqn;18 return (sk->out_phase_peeked >> 16) * s->cfg.frame_size;19}20 21static void playback_prep_freqn(struct usb_stream_kernel *sk, struct urb *urb)22{23 struct usb_stream *s = sk->s;24 int pack, lb = 0;25 26 for (pack = 0; pack < sk->n_o_ps; pack++) {27 int l = usb_stream_next_packet_size(sk);28 29 if (s->idle_outsize + lb + l > s->period_size)30 goto check;31 32 sk->out_phase = sk->out_phase_peeked;33 urb->iso_frame_desc[pack].offset = lb;34 urb->iso_frame_desc[pack].length = l;35 lb += l;36 }37 38check:39 urb->number_of_packets = pack;40 urb->transfer_buffer_length = lb;41 s->idle_outsize += lb - s->period_size;42}43 44static int init_pipe_urbs(struct usb_stream_kernel *sk,45 unsigned int use_packsize,46 struct urb **urbs, char *transfer,47 struct usb_device *dev, int pipe)48{49 int u, p;50 int maxpacket = use_packsize ?51 use_packsize : usb_maxpacket(dev, pipe);52 int transfer_length = maxpacket * sk->n_o_ps;53 54 for (u = 0; u < USB_STREAM_NURBS;55 ++u, transfer += transfer_length) {56 struct urb *urb = urbs[u];57 struct usb_iso_packet_descriptor *desc;58 59 urb->transfer_buffer = transfer;60 urb->dev = dev;61 urb->pipe = pipe;62 urb->number_of_packets = sk->n_o_ps;63 urb->context = sk;64 urb->interval = 1;65 if (usb_pipeout(pipe))66 continue;67 if (usb_urb_ep_type_check(urb))68 return -EINVAL;69 70 urb->transfer_buffer_length = transfer_length;71 desc = urb->iso_frame_desc;72 desc->offset = 0;73 desc->length = maxpacket;74 for (p = 1; p < sk->n_o_ps; ++p) {75 desc[p].offset = desc[p - 1].offset + maxpacket;76 desc[p].length = maxpacket;77 }78 }79 80 return 0;81}82 83static int init_urbs(struct usb_stream_kernel *sk, unsigned int use_packsize,84 struct usb_device *dev, int in_pipe, int out_pipe)85{86 struct usb_stream *s = sk->s;87 char *indata =88 (char *)s + sizeof(*s) + sizeof(struct usb_stream_packet) * s->inpackets;89 int u;90 91 for (u = 0; u < USB_STREAM_NURBS; ++u) {92 sk->inurb[u] = usb_alloc_urb(sk->n_o_ps, GFP_KERNEL);93 if (!sk->inurb[u])94 return -ENOMEM;95 96 sk->outurb[u] = usb_alloc_urb(sk->n_o_ps, GFP_KERNEL);97 if (!sk->outurb[u])98 return -ENOMEM;99 }100 101 if (init_pipe_urbs(sk, use_packsize, sk->inurb, indata, dev, in_pipe) ||102 init_pipe_urbs(sk, use_packsize, sk->outurb, sk->write_page, dev,103 out_pipe))104 return -EINVAL;105 106 return 0;107}108 109/*110 * convert a sampling rate into our full speed format (fs/1000 in Q16.16)111 * this will overflow at approx 524 kHz112 */113static inline unsigned int get_usb_full_speed_rate(unsigned int rate)114{115 return ((rate << 13) + 62) / 125;116}117 118/*119 * convert a sampling rate into USB high speed format (fs/8000 in Q16.16)120 * this will overflow at approx 4 MHz121 */122static inline unsigned int get_usb_high_speed_rate(unsigned int rate)123{124 return ((rate << 10) + 62) / 125;125}126 127void usb_stream_free(struct usb_stream_kernel *sk)128{129 struct usb_stream *s;130 unsigned int u;131 132 for (u = 0; u < USB_STREAM_NURBS; ++u) {133 usb_free_urb(sk->inurb[u]);134 sk->inurb[u] = NULL;135 usb_free_urb(sk->outurb[u]);136 sk->outurb[u] = NULL;137 }138 139 s = sk->s;140 if (!s)141 return;142 143 if (sk->write_page) {144 free_pages_exact(sk->write_page, s->write_size);145 sk->write_page = NULL;146 }147 148 free_pages_exact(s, s->read_size);149 sk->s = NULL;150}151 152struct usb_stream *usb_stream_new(struct usb_stream_kernel *sk,153 struct usb_device *dev,154 unsigned int in_endpoint,155 unsigned int out_endpoint,156 unsigned int sample_rate,157 unsigned int use_packsize,158 unsigned int period_frames,159 unsigned int frame_size)160{161 int packets, max_packsize;162 int in_pipe, out_pipe;163 int read_size = sizeof(struct usb_stream);164 int write_size;165 int usb_frames = dev->speed == USB_SPEED_HIGH ? 8000 : 1000;166 167 in_pipe = usb_rcvisocpipe(dev, in_endpoint);168 out_pipe = usb_sndisocpipe(dev, out_endpoint);169 170 max_packsize = use_packsize ?171 use_packsize : usb_maxpacket(dev, in_pipe);172 173 /*174 t_period = period_frames / sample_rate175 iso_packs = t_period / t_iso_frame176 = (period_frames / sample_rate) * (1 / t_iso_frame)177 */178 179 packets = period_frames * usb_frames / sample_rate + 1;180 181 if (dev->speed == USB_SPEED_HIGH)182 packets = (packets + 7) & ~7;183 184 read_size += packets * USB_STREAM_URBDEPTH *185 (max_packsize + sizeof(struct usb_stream_packet));186 187 max_packsize = usb_maxpacket(dev, out_pipe);188 write_size = max_packsize * packets * USB_STREAM_URBDEPTH;189 190 if (read_size >= 256*PAGE_SIZE || write_size >= 256*PAGE_SIZE) {191 dev_warn(&dev->dev, "%s: a size exceeds 128*PAGE_SIZE\n", __func__);192 goto out;193 }194 195 sk->s = alloc_pages_exact(read_size,196 GFP_KERNEL | __GFP_ZERO | __GFP_NOWARN);197 if (!sk->s) {198 dev_warn(&dev->dev, "us122l: couldn't allocate read buffer\n");199 goto out;200 }201 sk->s->cfg.version = USB_STREAM_INTERFACE_VERSION;202 203 sk->s->read_size = read_size;204 205 sk->s->cfg.sample_rate = sample_rate;206 sk->s->cfg.frame_size = frame_size;207 sk->n_o_ps = packets;208 sk->s->inpackets = packets * USB_STREAM_URBDEPTH;209 sk->s->cfg.period_frames = period_frames;210 sk->s->period_size = frame_size * period_frames;211 212 sk->s->write_size = write_size;213 214 sk->write_page = alloc_pages_exact(write_size,215 GFP_KERNEL | __GFP_ZERO | __GFP_NOWARN);216 if (!sk->write_page) {217 dev_warn(&dev->dev, "us122l: couldn't allocate write buffer\n");218 usb_stream_free(sk);219 return NULL;220 }221 222 /* calculate the frequency in 16.16 format */223 if (dev->speed == USB_SPEED_FULL)224 sk->freqn = get_usb_full_speed_rate(sample_rate);225 else226 sk->freqn = get_usb_high_speed_rate(sample_rate);227 228 if (init_urbs(sk, use_packsize, dev, in_pipe, out_pipe) < 0) {229 usb_stream_free(sk);230 return NULL;231 }232 233 sk->s->state = usb_stream_stopped;234out:235 return sk->s;236}237 238/* start */239 240static bool balance_check(struct usb_stream_kernel *sk, struct urb *urb)241{242 bool r;243 244 if (unlikely(urb->status)) {245 if (urb->status != -ESHUTDOWN && urb->status != -ENOENT)246 dev_warn(&sk->dev->dev, "%s: status=%i\n", __func__,247 urb->status);248 sk->iso_frame_balance = 0x7FFFFFFF;249 return false;250 }251 r = sk->iso_frame_balance == 0;252 if (!r)253 sk->i_urb = urb;254 return r;255}256 257static bool balance_playback(struct usb_stream_kernel *sk, struct urb *urb)258{259 sk->iso_frame_balance += urb->number_of_packets;260 return balance_check(sk, urb);261}262 263static bool balance_capture(struct usb_stream_kernel *sk, struct urb *urb)264{265 sk->iso_frame_balance -= urb->number_of_packets;266 return balance_check(sk, urb);267}268 269static void subs_set_complete(struct urb **urbs, void (*complete)(struct urb *))270{271 int u;272 273 for (u = 0; u < USB_STREAM_NURBS; u++) {274 struct urb *urb = urbs[u];275 276 urb->complete = complete;277 }278}279 280static int usb_stream_prepare_playback(struct usb_stream_kernel *sk,281 struct urb *inurb)282{283 struct usb_stream *s = sk->s;284 struct urb *io;285 struct usb_iso_packet_descriptor *id, *od;286 int p = 0, lb = 0, l = 0;287 288 io = sk->idle_outurb;289 od = io->iso_frame_desc;290 291 for (; s->sync_packet < 0; ++p, ++s->sync_packet) {292 struct urb *ii = sk->completed_inurb;293 294 id = ii->iso_frame_desc +295 ii->number_of_packets + s->sync_packet;296 l = id->actual_length;297 298 od[p].length = l;299 od[p].offset = lb;300 lb += l;301 }302 303 for (;304 s->sync_packet < inurb->number_of_packets && p < sk->n_o_ps;305 ++p, ++s->sync_packet) {306 l = inurb->iso_frame_desc[s->sync_packet].actual_length;307 308 if (s->idle_outsize + lb + l > s->period_size)309 goto check_ok;310 311 od[p].length = l;312 od[p].offset = lb;313 lb += l;314 }315 316check_ok:317 s->sync_packet -= inurb->number_of_packets;318 if (unlikely(s->sync_packet < -2 || s->sync_packet > 0)) {319 dev_warn(&sk->dev->dev,320 "%s: invalid sync_packet = %i; p=%i nop=%i %i %x %x %x > %x\n",321 __func__,322 s->sync_packet, p, inurb->number_of_packets,323 s->idle_outsize + lb + l,324 s->idle_outsize, lb, l,325 s->period_size);326 return -1;327 }328 if (unlikely(lb % s->cfg.frame_size)) {329 dev_warn(&sk->dev->dev, "%s: invalid outsize = %i\n",330 __func__, lb);331 return -1;332 }333 s->idle_outsize += lb - s->period_size;334 io->number_of_packets = p;335 io->transfer_buffer_length = lb;336 if (s->idle_outsize <= 0)337 return 0;338 339 dev_warn(&sk->dev->dev, "%s: idle=%i\n", __func__, s->idle_outsize);340 return -1;341}342 343static void prepare_inurb(int number_of_packets, struct urb *iu)344{345 struct usb_iso_packet_descriptor *id;346 int p;347 348 iu->number_of_packets = number_of_packets;349 id = iu->iso_frame_desc;350 id->offset = 0;351 for (p = 0; p < iu->number_of_packets - 1; ++p)352 id[p + 1].offset = id[p].offset + id[p].length;353 354 iu->transfer_buffer_length =355 id[0].length * iu->number_of_packets;356}357 358static int submit_urbs(struct usb_stream_kernel *sk,359 struct urb *inurb, struct urb *outurb)360{361 int err;362 363 prepare_inurb(sk->idle_outurb->number_of_packets, sk->idle_inurb);364 err = usb_submit_urb(sk->idle_inurb, GFP_ATOMIC);365 if (err < 0)366 goto report_failure;367 368 sk->idle_inurb = sk->completed_inurb;369 sk->completed_inurb = inurb;370 err = usb_submit_urb(sk->idle_outurb, GFP_ATOMIC);371 if (err < 0)372 goto report_failure;373 374 sk->idle_outurb = sk->completed_outurb;375 sk->completed_outurb = outurb;376 return 0;377 378report_failure:379 dev_err(&sk->dev->dev, "%s error: %i\n", __func__, err);380 return err;381}382 383#ifdef DEBUG_LOOP_BACK384/*385 This loop_back() shows how to read/write the period data.386 */387static void loop_back(struct usb_stream *s)388{389 char *i, *o;390 int il, ol, l, p;391 struct urb *iu;392 struct usb_iso_packet_descriptor *id;393 394 o = s->playback1st_to;395 ol = s->playback1st_size;396 l = 0;397 398 if (s->insplit_pack >= 0) {399 iu = sk->idle_inurb;400 id = iu->iso_frame_desc;401 p = s->insplit_pack;402 } else403 goto second;404loop:405 for (; p < iu->number_of_packets && l < s->period_size; ++p) {406 i = iu->transfer_buffer + id[p].offset;407 il = id[p].actual_length;408 if (l + il > s->period_size)409 il = s->period_size - l;410 if (il <= ol) {411 memcpy(o, i, il);412 o += il;413 ol -= il;414 } else {415 memcpy(o, i, ol);416 singen_6pack(o, ol);417 o = s->playback_to;418 memcpy(o, i + ol, il - ol);419 o += il - ol;420 ol = s->period_size - s->playback1st_size;421 }422 l += il;423 }424 if (iu == sk->completed_inurb) {425 if (l != s->period_size)426 dev_dbg(&sk->dev->dev, "%s:%i %i\n", __func__, __LINE__,427 l/(int)s->cfg.frame_size);428 429 return;430 }431second:432 iu = sk->completed_inurb;433 id = iu->iso_frame_desc;434 p = 0;435 goto loop;436 437}438#else439static void loop_back(struct usb_stream *s)440{441}442#endif443 444static void stream_idle(struct usb_stream_kernel *sk,445 struct urb *inurb, struct urb *outurb)446{447 struct usb_stream *s = sk->s;448 int l, p;449 int insize = s->idle_insize;450 int urb_size = 0;451 452 s->inpacket_split = s->next_inpacket_split;453 s->inpacket_split_at = s->next_inpacket_split_at;454 s->next_inpacket_split = -1;455 s->next_inpacket_split_at = 0;456 457 for (p = 0; p < inurb->number_of_packets; ++p) {458 struct usb_iso_packet_descriptor *id = inurb->iso_frame_desc;459 460 l = id[p].actual_length;461 if (unlikely(l == 0 || id[p].status)) {462 dev_warn(&sk->dev->dev, "%s: underrun, status=%u\n",463 __func__, id[p].status);464 goto err_out;465 }466 s->inpacket_head++;467 s->inpacket_head %= s->inpackets;468 if (s->inpacket_split == -1)469 s->inpacket_split = s->inpacket_head;470 471 s->inpacket[s->inpacket_head].offset =472 id[p].offset + (inurb->transfer_buffer - (void *)s);473 s->inpacket[s->inpacket_head].length = l;474 if (insize + l > s->period_size &&475 s->next_inpacket_split == -1) {476 s->next_inpacket_split = s->inpacket_head;477 s->next_inpacket_split_at = s->period_size - insize;478 }479 insize += l;480 urb_size += l;481 }482 s->idle_insize += urb_size - s->period_size;483 if (s->idle_insize < 0) {484 dev_warn(&sk->dev->dev, "%s error: %i\n", __func__,485 (s->idle_insize)/(int)s->cfg.frame_size);486 goto err_out;487 }488 s->insize_done += urb_size;489 490 l = s->idle_outsize;491 s->outpacket[0].offset = (sk->idle_outurb->transfer_buffer -492 sk->write_page) - l;493 494 if (usb_stream_prepare_playback(sk, inurb) < 0)495 goto err_out;496 497 s->outpacket[0].length = sk->idle_outurb->transfer_buffer_length + l;498 s->outpacket[1].offset = sk->completed_outurb->transfer_buffer -499 sk->write_page;500 501 if (submit_urbs(sk, inurb, outurb) < 0)502 goto err_out;503 504 loop_back(s);505 s->periods_done++;506 wake_up_all(&sk->sleep);507 return;508err_out:509 s->state = usb_stream_xrun;510 wake_up_all(&sk->sleep);511}512 513static void i_capture_idle(struct urb *urb)514{515 struct usb_stream_kernel *sk = urb->context;516 517 if (balance_capture(sk, urb))518 stream_idle(sk, urb, sk->i_urb);519}520 521static void i_playback_idle(struct urb *urb)522{523 struct usb_stream_kernel *sk = urb->context;524 525 if (balance_playback(sk, urb))526 stream_idle(sk, sk->i_urb, urb);527}528 529static void stream_start(struct usb_stream_kernel *sk,530 struct urb *inurb, struct urb *outurb)531{532 struct usb_stream *s = sk->s;533 534 if (s->state >= usb_stream_sync1) {535 int l, p, max_diff, max_diff_0;536 int urb_size = 0;537 unsigned int frames_per_packet, min_frames = 0;538 539 frames_per_packet = (s->period_size - s->idle_insize);540 frames_per_packet <<= 8;541 frames_per_packet /=542 s->cfg.frame_size * inurb->number_of_packets;543 frames_per_packet++;544 545 max_diff_0 = s->cfg.frame_size;546 if (s->cfg.period_frames >= 256)547 max_diff_0 <<= 1;548 if (s->cfg.period_frames >= 1024)549 max_diff_0 <<= 1;550 max_diff = max_diff_0;551 for (p = 0; p < inurb->number_of_packets; ++p) {552 int diff;553 554 l = inurb->iso_frame_desc[p].actual_length;555 urb_size += l;556 557 min_frames += frames_per_packet;558 diff = urb_size -559 (min_frames >> 8) * s->cfg.frame_size;560 if (diff < max_diff)561 max_diff = diff;562 }563 s->idle_insize -= max_diff - max_diff_0;564 s->idle_insize += urb_size - s->period_size;565 if (s->idle_insize < 0) {566 dev_warn(&sk->dev->dev, "%s idle_insize: %i %i %i\n",567 __func__,568 s->idle_insize, urb_size, s->period_size);569 return;570 } else if (s->idle_insize == 0) {571 s->next_inpacket_split =572 (s->inpacket_head + 1) % s->inpackets;573 s->next_inpacket_split_at = 0;574 } else {575 unsigned int split = s->inpacket_head;576 577 l = s->idle_insize;578 while (l > s->inpacket[split].length) {579 l -= s->inpacket[split].length;580 if (split == 0)581 split = s->inpackets - 1;582 else583 split--;584 }585 s->next_inpacket_split = split;586 s->next_inpacket_split_at =587 s->inpacket[split].length - l;588 }589 590 s->insize_done += urb_size;591 592 if (usb_stream_prepare_playback(sk, inurb) < 0)593 return;594 595 } else596 playback_prep_freqn(sk, sk->idle_outurb);597 598 if (submit_urbs(sk, inurb, outurb) < 0)599 return;600 601 if (s->state == usb_stream_sync1 && s->insize_done > 360000) {602 /* just guesswork ^^^^^^ */603 s->state = usb_stream_ready;604 subs_set_complete(sk->inurb, i_capture_idle);605 subs_set_complete(sk->outurb, i_playback_idle);606 }607}608 609static void i_capture_start(struct urb *urb)610{611 struct usb_iso_packet_descriptor *id = urb->iso_frame_desc;612 struct usb_stream_kernel *sk = urb->context;613 struct usb_stream *s = sk->s;614 int p;615 int empty = 0;616 617 if (urb->status) {618 dev_warn(&sk->dev->dev, "%s: status=%i\n", __func__, urb->status);619 return;620 }621 622 for (p = 0; p < urb->number_of_packets; ++p) {623 int l = id[p].actual_length;624 625 if (l < s->cfg.frame_size) {626 ++empty;627 if (s->state >= usb_stream_sync0) {628 dev_warn(&sk->dev->dev, "%s: length %i\n", __func__, l);629 return;630 }631 }632 s->inpacket_head++;633 s->inpacket_head %= s->inpackets;634 s->inpacket[s->inpacket_head].offset =635 id[p].offset + (urb->transfer_buffer - (void *)s);636 s->inpacket[s->inpacket_head].length = l;637 }638#ifdef SHOW_EMPTY639 if (empty) {640 dev_dbg(&sk->dev->dev, "%s:%i: %i", __func__, __LINE__,641 urb->iso_frame_desc[0].actual_length);642 for (pack = 1; pack < urb->number_of_packets; ++pack) {643 int l = urb->iso_frame_desc[pack].actual_length;644 645 printk(KERN_CONT " %i", l);646 }647 printk(KERN_CONT "\n");648 }649#endif650 if (!empty && s->state < usb_stream_sync1)651 ++s->state;652 653 if (balance_capture(sk, urb))654 stream_start(sk, urb, sk->i_urb);655}656 657static void i_playback_start(struct urb *urb)658{659 struct usb_stream_kernel *sk = urb->context;660 661 if (balance_playback(sk, urb))662 stream_start(sk, sk->i_urb, urb);663}664 665int usb_stream_start(struct usb_stream_kernel *sk)666{667 struct usb_stream *s = sk->s;668 int frame = 0;669 int u, err;670 int try = 0;671 672 if (s->state != usb_stream_stopped)673 return -EAGAIN;674 675 subs_set_complete(sk->inurb, i_capture_start);676 subs_set_complete(sk->outurb, i_playback_start);677 memset(sk->write_page, 0, s->write_size);678dotry:679 s->insize_done = 0;680 s->idle_insize = 0;681 s->idle_outsize = 0;682 s->sync_packet = -1;683 s->inpacket_head = -1;684 sk->iso_frame_balance = 0;685 ++try;686 for (u = 0; u < 2; u++) {687 struct urb *inurb = sk->inurb[u];688 struct urb *outurb = sk->outurb[u];689 690 playback_prep_freqn(sk, outurb);691 inurb->number_of_packets = outurb->number_of_packets;692 inurb->transfer_buffer_length =693 inurb->number_of_packets *694 inurb->iso_frame_desc[0].length;695 696 if (u == 0) {697 int now;698 struct usb_device *dev = inurb->dev;699 700 frame = usb_get_current_frame_number(dev);701 do {702 now = usb_get_current_frame_number(dev);703 } while (now > -1 && now == frame);704 }705 err = usb_submit_urb(inurb, GFP_ATOMIC);706 if (err < 0) {707 dev_err(&sk->dev->dev,708 "%s: usb_submit_urb(sk->inurb[%i]) returned %i\n",709 __func__, u, err);710 return err;711 }712 err = usb_submit_urb(outurb, GFP_ATOMIC);713 if (err < 0) {714 dev_err(&sk->dev->dev,715 "%s: usb_submit_urb(sk->outurb[%i]) returned %i\n",716 __func__, u, err);717 return err;718 }719 720 if (inurb->start_frame != outurb->start_frame) {721 dev_dbg(&sk->dev->dev,722 "%s: u[%i] start_frames differ in:%u out:%u\n",723 __func__, u, inurb->start_frame, outurb->start_frame);724 goto check_retry;725 }726 }727 try = 0;728check_retry:729 if (try) {730 usb_stream_stop(sk);731 if (try < 5) {732 msleep(1500);733 dev_dbg(&sk->dev->dev, "goto dotry;\n");734 goto dotry;735 }736 dev_warn(&sk->dev->dev,737 "%s: couldn't start all urbs on the same start_frame.\n",738 __func__);739 return -EFAULT;740 }741 742 sk->idle_inurb = sk->inurb[USB_STREAM_NURBS - 2];743 sk->idle_outurb = sk->outurb[USB_STREAM_NURBS - 2];744 sk->completed_inurb = sk->inurb[USB_STREAM_NURBS - 1];745 sk->completed_outurb = sk->outurb[USB_STREAM_NURBS - 1];746 747/* wait, check */748 {749 int wait_ms = 3000;750 751 while (s->state != usb_stream_ready && wait_ms > 0) {752 msleep(200);753 wait_ms -= 200;754 }755 }756 757 return s->state == usb_stream_ready ? 0 : -EFAULT;758}759 760 761/* stop */762 763void usb_stream_stop(struct usb_stream_kernel *sk)764{765 int u;766 767 if (!sk->s)768 return;769 for (u = 0; u < USB_STREAM_NURBS; ++u) {770 usb_kill_urb(sk->inurb[u]);771 usb_kill_urb(sk->outurb[u]);772 }773 sk->s->state = usb_stream_stopped;774 msleep(400);775}776