1 /* 2 * Copyright (c) 1999 Cameron Grant <gandalf@vilnya.demon.co.uk> 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions 7 * are met: 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24 * SUCH DAMAGE. 25 * 26 * $FreeBSD$ 27 */ 28 29 #include <dev/sound/pcm/sound.h> 30 31 #include "feeder_if.h" 32 33 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder"); 34 35 #define FEEDBUFSZ 8192 36 #undef FEEDER_DEBUG 37 38 struct feed_rate_info { 39 u_int32_t src, dst; 40 int srcpos, srcinc; 41 int16_t *buffer; 42 u_int16_t alpha; 43 }; 44 45 static int 46 feed_rate_setup(struct pcm_feeder *f) 47 { 48 struct feed_rate_info *info = f->data; 49 50 info->srcinc = (info->src << 16) / info->dst; 51 /* srcinc is 16.16 fixed point increment for srcpos for each dstpos */ 52 info->srcpos = 0; 53 return 0; 54 } 55 56 static int 57 feed_rate_set(struct pcm_feeder *f, int what, int value) 58 { 59 struct feed_rate_info *info = f->data; 60 61 switch(what) { 62 case FEEDRATE_SRC: 63 info->src = value; 64 break; 65 case FEEDRATE_DST: 66 info->dst = value; 67 break; 68 default: 69 return -1; 70 } 71 return feed_rate_setup(f); 72 } 73 74 static int 75 feed_rate_get(struct pcm_feeder *f, int what) 76 { 77 struct feed_rate_info *info = f->data; 78 79 switch(what) { 80 case FEEDRATE_SRC: 81 return info->src; 82 case FEEDRATE_DST: 83 return info->dst; 84 default: 85 return -1; 86 } 87 return -1; 88 } 89 90 static int 91 feed_rate_init(struct pcm_feeder *f) 92 { 93 struct feed_rate_info *info; 94 95 info = malloc(sizeof(*info), M_RATEFEEDER, M_WAITOK | M_ZERO); 96 if (info == NULL) 97 return ENOMEM; 98 info->buffer = malloc(FEEDBUFSZ, M_RATEFEEDER, M_WAITOK | M_ZERO); 99 if (info->buffer == NULL) { 100 free(info, M_RATEFEEDER); 101 return ENOMEM; 102 } 103 info->src = DSP_DEFAULT_SPEED; 104 info->dst = DSP_DEFAULT_SPEED; 105 info->alpha = 0; 106 f->data = info; 107 return feed_rate_setup(f); 108 } 109 110 static int 111 feed_rate_free(struct pcm_feeder *f) 112 { 113 struct feed_rate_info *info = f->data; 114 115 if (info) { 116 if (info->buffer) 117 free(info->buffer, M_RATEFEEDER); 118 free(info, M_RATEFEEDER); 119 } 120 f->data = NULL; 121 return 0; 122 } 123 124 static int 125 feed_rate(struct pcm_feeder *f, struct pcm_channel *c, u_int8_t *b, u_int32_t count, void *source) 126 { 127 struct feed_rate_info *info = f->data; 128 int16_t *destbuf = (int16_t *)b; 129 int fetch, v, alpha, hidelta, spos, dpos; 130 131 /* 132 * at this point: 133 * info->srcpos is 24.8 fixed offset into the fetchbuffer. 0 <= srcpos <= 0xff 134 * 135 * our input and output are always AFMT_S16LE stereo. this simplifies things. 136 */ 137 138 /* 139 * we start by fetching enough source data into our buffer to generate 140 * about as much as was requested. we put it at offset 2 in the 141 * buffer so that we can interpolate from the last samples in the 142 * previous iteration- when we finish we will move our last samples 143 * to the start of the buffer. 144 */ 145 spos = 0; 146 dpos = 0; 147 148 /* fetch is in bytes */ 149 fetch = (count * info->srcinc) >> 16; 150 fetch = min(fetch, FEEDBUFSZ - 4) & ~3; 151 if (fetch == 0) 152 return 0; 153 fetch = FEEDER_FEED(f->source, c, ((u_int8_t *)info->buffer) + 4, fetch, source); 154 fetch /= 2; 155 156 alpha = info->alpha; 157 hidelta = min(info->srcinc >> 16, 1) * 2; 158 while ((spos + hidelta + 1) < fetch) { 159 v = (info->buffer[spos] * (0xffff - alpha)) + (info->buffer[spos + hidelta] * alpha); 160 destbuf[dpos++] = v >> 16; 161 162 v = (info->buffer[spos + 1] * (0xffff - alpha)) + (info->buffer[spos + hidelta + 1] * alpha); 163 destbuf[dpos++] = v >> 16; 164 165 alpha += info->srcinc; 166 spos += (alpha >> 16) * 2; 167 alpha &= 0xffff; 168 169 } 170 info->alpha = alpha & 0xffff; 171 info->buffer[0] = info->buffer[spos - hidelta]; 172 info->buffer[1] = info->buffer[spos - hidelta + 1]; 173 174 count = dpos * 2; 175 return count; 176 } 177 178 static struct pcm_feederdesc feeder_rate_desc[] = { 179 {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0}, 180 {0}, 181 }; 182 static kobj_method_t feeder_rate_methods[] = { 183 KOBJMETHOD(feeder_init, feed_rate_init), 184 KOBJMETHOD(feeder_free, feed_rate_free), 185 KOBJMETHOD(feeder_set, feed_rate_set), 186 KOBJMETHOD(feeder_get, feed_rate_get), 187 KOBJMETHOD(feeder_feed, feed_rate), 188 { 0, 0 } 189 }; 190 FEEDER_DECLARE(feeder_rate, 2, NULL); 191 192 193