move midi-control into separate function&file

.gitignore

@@ -0,0 +1,2 @@
+build
+*~

controller.c

@@ -0,0 +1,110 @@
+#include "pipewire/keys.h"
+#include "pipewire/port.h"
+#include "spa/pod/iter.h"
+#include "spa/utils/defs.h"
+#include <signal.h>
+#include <stdio.h>
+
+#include <spa/control/control.h>
+#include <spa/param/latency-utils.h>
+#include <spa/pod/builder.h>
+#include <spa/pod/pod.h>
+
+#include <pipewire/filter.h>
+#include <pipewire/pipewire.h>
+
+#include "guitfx.h"
+#include "delay.h"
+#include "sust.h"
+#include "gate.h"
+
+void midi_control(
+		  struct FxData * data,
+
+		  uint64_t frame,
+		  uint64_t offset,
+		  unsigned sec,
+		  unsigned midi_size,
+		  uint8_t * midi_data,
+		  struct spa_io_position* position
+) {
+  printf("[%d] MIDI message (%d bytes) : %x, %x, %x\n", sec, midi_size,
+	 midi_data[0], midi_data[1], midi_data[2]);
+
+                        switch (midi_data[0] & 0xff) {
+                        case 0xb0:
+                            switch (midi_data[1]) {
+                            case 0x0b:
+                                // expr pedal
+                                float val_f = ((float)midi_data[2]) / 128.0;
+
+                                float thres = 0.5;
+
+                                if (val_f > thres) {
+                                    float expr_mix = (val_f - thres) / (1.0 - thres);
+                                    printf("Expr Pedal %f\n", expr_mix);
+
+                                    data->delay.mix = expr_mix;
+                                } else {
+                                    data->delay.mix = 0.0;
+                                }
+                                break;
+
+                            case 0x42:
+
+                                // noise gate calibration
+                                if (data->prog == 2) {
+                                    if (midi_data[2] >= 64) {
+                                        data->gate.threshold = data->gate.cur_block_sum * 0.8;
+                                        printf("calibrate noise gate: threshold = %f\n",
+                                            data->gate.threshold);
+                                    }
+                                }
+
+                                // sust pedal
+                                if (data->prog == 1) {
+                                    if (midi_data[2] >= 64) {
+                                        sust_swap(&data->sust);
+                                        data->sust.playing = true;
+
+                                        data->sust.start_idx = data->sust.idx;
+                                        data->sust.idx = 0;
+                                    } else {
+                                        data->sust.playing = false;
+                                    }
+                                }
+
+                                // tap tempo
+                                if (data->prog == 0) {
+                                    uint64_t cur_tap = frame + offset;
+                                    uint64_t duration = cur_tap - data->last_tap;
+                                    data->last_tap = cur_tap;
+                                    if (duration < (4 * position->clock.rate.denom)) {
+                                        delay_set_time(&data->delay, duration);
+                                    }
+
+                                    sust_resize(&data->sust, duration);
+                                }
+
+                                break;
+                            }
+
+                            break;
+
+                        case 0xc0:
+                            // program change
+                            if (midi_data[2] >= 64) {
+                                sust_swap(&data->sust);
+                                data->sust.playing = true;
+
+                                data->sust.start_idx = data->sust.idx;
+                                data->sust.idx = 0;
+                            } else {
+                                data->sust.playing = false;
+                            }
+                            printf("program change: %u\n", midi_data[1]);
+                            data->prog = midi_data[1];
+                            break;
+                        }
+}
+

delay.c

@@ -1,12 +1,12 @@
-#include <stdint.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <stdio.h>
 #include "delay.h"
+#include <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
 
 void delay_init(
-    struct delay * delay
-) {
+    struct delay* delay)
+{
     delay->mix = 0.8;
     delay->feedback = 0.8;
     delay->duration = 0;
@@ -16,39 +16,38 @@ void delay_init(
 }
 
 void delay_set_time(
-    struct delay * delay,
-    uint64_t new_duration
-) {
+    struct delay* delay,
+    uint64_t new_duration)
+{
     printf("set delay duration to %lu samples\n", new_duration);
-    if( new_duration > delay->buf_capacity ) {
+    if (new_duration > delay->buf_capacity) {
         delay->buf_capacity = new_duration;
-        delay->buf = realloc( delay->buf, sizeof(float) * new_duration );
+        delay->buf = realloc(delay->buf, sizeof(float) * new_duration);
     }
 
-    for( int i = delay->duration; i < new_duration; ++i ) {
+    for (int i = delay->duration; i < new_duration; ++i) {
         delay->buf[i] = delay->buf[i - delay->duration];
     }
 
     delay->duration = new_duration;
-    if( new_duration > 0 ) {
+    if (new_duration > 0) {
         delay->buf_idx %= new_duration;
     }
 }
 
 void delay_process(
-    struct delay * delay,
+    struct delay* delay,
     size_t frame_size,
-    float const * in,
-    float * out
-) {
-    for( size_t i = 0; i < frame_size; ++i ) {
-        if( delay->duration > 0 ) {
-            out[i] =
-                0.5 * in[i]
-                + 0.5 * delay->mix * delay->buf[ delay->buf_idx ];
+    float const* in,
+    float* out)
+{
+    for (size_t i = 0; i < frame_size; ++i) {
+        if (delay->duration > 0) {
+            out[i] = in[i]
+                + delay->mix * delay->buf[delay->buf_idx];
 
-            delay->buf[ delay->buf_idx ] *= delay->feedback;
-            delay->buf[ delay->buf_idx ] += (1.0 - delay->feedback) * in[i];
+            delay->buf[delay->buf_idx] *= delay->feedback;
+            delay->buf[delay->buf_idx] += (1.0 - delay->feedback) * in[i];
             delay->buf_idx = (delay->buf_idx + 1) % delay->duration;
         } else {
             out[i] = 0.5 * in[i];

delay.h

@@ -1,7 +1,7 @@
 #pragma once
 
-#include <stdint.h>
 #include <stddef.h>
+#include <stdint.h>
 
 struct delay {
     uint64_t duration;
@@ -10,21 +10,18 @@ struct delay {
 
     uint64_t buf_idx;
     size_t buf_capacity;
-    float * buf;
+    float* buf;
 };
 
 void delay_init(
-    struct delay * delay
-);
+    struct delay* delay);
 
 void delay_set_time(
-    struct delay * delay,
-    uint64_t new_duration
-);
+    struct delay* delay,
+    uint64_t new_duration);
 
 void delay_process(
-    struct delay * delay,
+    struct delay* delay,
     size_t frame_size,
-    float const * in,
-    float * out
-);
+    float const* in,
+    float* out);

gate.c

@@ -0,0 +1,64 @@
+#include "gate.h"
+#include <math.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+void gate_init(
+    struct gate* gate)
+{
+    gate->threshold = 0.0;
+    gate->enable_calibration = false;
+
+    gate->block_size = 1024;
+    gate->cur_block_count = 0;
+    gate->cur_block_sum = 0.0;
+
+    gate->is_active = false;
+    gate->cur_gain = 1.0;
+    gate->attack = 1.0 / 128.0;
+    gate->release = 1.0 / 8192.0;
+}
+
+void gate_process(
+    struct gate* gate,
+    size_t frame_size,
+    float const* in,
+    float* out)
+{
+
+    bool act = false;
+
+    for (size_t i = 0; i < frame_size; ++i) {
+        gate->cur_block_sum += fabs(in[i]);
+        gate->cur_block_count += 1;
+
+        if (gate->cur_block_count >= gate->block_size) {
+            // printf("new block sum %f, gain = %f\n", gate->cur_block_sum, gate->cur_gain);
+
+            gate->last_block_sum = gate->cur_block_sum;
+            gate->cur_block_sum = 0.0;
+
+            if (gate->last_block_sum > 0.03) {
+                act = true;
+            } else {
+                act = false;
+            }
+        }
+
+        if (act) {
+            gate->cur_gain += gate->attack;
+            if (gate->cur_gain > 1.0) {
+                gate->cur_gain = 1.0;
+            }
+        } else {
+            gate->cur_gain -= gate->release;
+            if (gate->cur_gain < 0.0) {
+                gate->cur_gain = 0.0;
+            }
+        }
+
+        out[i] = in[i] * gate->cur_gain;
+    }
+}

gate.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+struct gate {
+    float threshold;
+    bool enable_calibration;
+
+    unsigned block_size;
+    unsigned cur_block_count;
+    float cur_block_sum;
+    float last_block_sum;
+
+    bool is_active;
+    float cur_gain;
+
+    float attack;
+    float release;
+};
+
+void gate_init();
+void gate_process(
+    struct gate* gate,
+    size_t frame_size,
+    float const* in,
+    float* out);

guitfx.c

@@ -2,223 +2,186 @@
 #include "pipewire/port.h"
 #include "spa/pod/iter.h"
 #include "spa/utils/defs.h"
-#include <stdio.h>
 #include <signal.h>
+#include <stdio.h>
 
-#include <spa/pod/pod.h>
-#include <spa/pod/builder.h>
 #include <spa/control/control.h>
 #include <spa/param/latency-utils.h>
+#include <spa/pod/builder.h>
+#include <spa/pod/pod.h>
 
-#include <pipewire/pipewire.h>
 #include <pipewire/filter.h>
+#include <pipewire/pipewire.h>
 
 #include "delay.h"
+#include "gate.h"
+#include "sust.h"
+#include "guitfx.h"
+
+float envelope(float x);
 
 struct data;
 
 struct port {
-	struct data *data;
+    struct data* data;
 };
 
+void midi_control(
+		  struct FxData * data,
+
+		  uint64_t frame,
+		  uint64_t offset,
+		  unsigned sec,
+		  unsigned midi_size,
+		  uint8_t * midi_data,
+		  struct spa_io_position* position);
+
+
 struct data {
-	struct pw_main_loop * loop;
-	struct pw_filter * filter;
-	struct port * guit_in_port;
-	struct port * midi_in_port;
-	struct port * out_port;
-
-	//! effect data
-	uint64_t last_tap;
-	struct delay delay;
-
-	//! elapsed time in number of samples
-	uint64_t time;
+    struct pw_main_loop* loop;
+    struct pw_filter* filter;
+    struct port* guit_in_port;
+    struct port* midi_in_port;
+    struct port* out_port;
+    struct FxData fx_data;
 };
 
-static void on_process(void *userdata, struct spa_io_position *position)
+static void on_process(void* userdata, struct spa_io_position* position)
 {
-	struct data *data = userdata;
+    struct data* data = userdata;
 
-	uint32_t n_samples = position->clock.duration;
-	uint64_t frame = data->time;
-	data->time += n_samples;
+    uint32_t n_samples = position->clock.duration;
+    uint64_t frame = data->fx_data.time;
+    data->fx_data.time += n_samples;
 
-	struct pw_buffer * b = pw_filter_dequeue_buffer(data->midi_in_port);
-	if( b == NULL ) {
-	   fprintf(stderr, "on_process(): no buffer for midi_in_port\n");
-	   return;
-	}
+    struct pw_buffer* b = pw_filter_dequeue_buffer(data->midi_in_port);
+    if (b == NULL) {
+        fprintf(stderr, "on_process(): no buffer for midi_in_port\n");
+        return;
+    }
 
-	struct spa_buffer * buf = b->buffer;
-	spa_assert(buf->n_datas == 1);
-	struct spa_data   * d   = &buf->datas[0];
+    struct spa_buffer* buf = b->buffer;
+    spa_assert(buf->n_datas == 1);
+    struct spa_data* d = &buf->datas[0];
 
-	if( d->data ) {
-	    struct spa_pod * pod =
-			spa_pod_from_data(
-			    d->data,
-				d->maxsize,
-				d->chunk->offset,
-				d->chunk->size
-			);
+    if (d->data) {
+        struct spa_pod* pod = spa_pod_from_data(d->data, d->maxsize,
+            d->chunk->offset, d->chunk->size);
 
-		if( pod ) {
-		    if( spa_pod_is_sequence(pod) ) {
-				struct spa_pod_sequence * pod_seq = (struct spa_pod_sequence*) pod;
-				struct spa_pod_control * c;
-				SPA_POD_SEQUENCE_FOREACH(pod_seq, c) {
-				    if( c->type == SPA_CONTROL_Midi ) {
-						unsigned sec =
-						      (frame + c->offset)
-							/ (float) position->clock.rate.denom;
-						char * midi_data = SPA_POD_BODY(&c->value);
-						unsigned size = SPA_POD_BODY_SIZE(&c->value);
+        if (pod) {
+            if (spa_pod_is_sequence(pod)) {
+                struct spa_pod_sequence* pod_seq = (struct spa_pod_sequence*)pod;
+                struct spa_pod_control* c;
+                SPA_POD_SEQUENCE_FOREACH(pod_seq, c)
+                {
+                    if (c->type == SPA_CONTROL_Midi) {
+                        unsigned sec = (frame + c->offset) / (float)position->clock.rate.denom;
+                        char* midi_data = SPA_POD_BODY(&c->value);
+                        unsigned midi_size = SPA_POD_BODY_SIZE(&c->value);
 
-						printf("[%d] MIDI message (%d bytes) : %x, %x, %x\n", sec, size, midi_data[0], midi_data[1], midi_data[2]);
+			midi_control( &data->fx_data, frame, c->offset, sec, midi_size, midi_data, position );
+                    } else {
 
-						switch( midi_data[0] & 0xff ) {
-						    case 0xb0:
-								switch( midi_data[1] ) {
-									case 0x0b:
-									    // expr pedal
-										float val_f = ((float)midi_data[2]) / 128.0;
-										printf("Expr Pedal %f\n", val_f);
+		      printf("on_process(): non midi-control\n");
+                    }
+                }
+            } else {
+                fprintf(stderr, "on_process(): unexpected POD that is not a sequence "
+                                "(midi_in_port)\n");
+            }
+        } else {
+            fprintf(stderr, "on_process(): pod is NULL\n");
+        }
+    } else {
+        fprintf(stderr, "on_process(): no data in buffer of midi_in_port\n");
+    }
 
-										data->delay.mix = val_f;
-									    break;
+    pw_filter_queue_buffer(data->midi_in_port, b);
 
-								    case 0x42:
-										// sust pedal
-
-   									    uint64_t cur_tap = frame + c->offset;
-    									uint64_t duration = cur_tap - data->last_tap;
-    									data->last_tap = cur_tap;
-    									if( duration < (4*position->clock.rate.denom) ) {
-    									  delay_set_time( &data->delay, duration/4 );
-    									}
-										break;
-								}
-								break;
-
-							case 0xc0:
-							    // program change
-								printf("program change: %u\n", midi_data[1]);
-							    break;
-						}
-					} else {
-					    printf("on_process(): non midi-control\n");
-					}
-				}
-			} else {
-			    fprintf(stderr, "on_process(): unexpected POD that is not a sequence (midi_in_port)\n");
-			}
-		} else {
-		    fprintf(stderr, "on_process(): pod is NULL\n");
-		}
-	} else {
-	    fprintf(stderr, "on_process(): no data in buffer of midi_in_port\n");
-	}
-
-	pw_filter_queue_buffer(data->midi_in_port, b);
-
-
-	float * const in = pw_filter_get_dsp_buffer(data->guit_in_port, n_samples);
-	float * out = pw_filter_get_dsp_buffer(data->out_port, n_samples);
-	if( in && out ) {
-        delay_process( &data->delay, n_samples, in, out );
-	}
+    float* const in = pw_filter_get_dsp_buffer(data->guit_in_port, n_samples);
+    float tmp1[n_samples];
+    float tmp2[n_samples];
+    float* out = pw_filter_get_dsp_buffer(data->out_port, n_samples);
+    if (in && out) {
+        gate_process(&data->fx_data.gate, n_samples, in, tmp1);
+        sust_process(&data->fx_data.sust, n_samples, tmp1, tmp2);
+        delay_process(&data->fx_data.delay, n_samples, tmp2, out);
+    }
 }
 
 static const struct pw_filter_events filter_events = {
-	PW_VERSION_FILTER_EVENTS,
-	.process = on_process,
+    PW_VERSION_FILTER_EVENTS,
+    .process = on_process,
 };
 
-static void do_quit(void *userdata, int signal_number)
+static void do_quit(void* userdata, int signal_number)
 {
-	struct data *data = userdata;
-	pw_main_loop_quit(data->loop);
+    struct data* data = userdata;
+    pw_main_loop_quit(data->loop);
 }
 
-int main(int argc, char *argv[])
+int main(int argc, char* argv[])
 {
-	struct data data = { 0, };
+    struct data data = {
+        0,
+    };
 
-	delay_init( &data.delay );
+    gate_init(&data.fx_data.gate);
+    delay_init(&data.fx_data.delay);
+    sust_init(&data.fx_data.sust);
+    data.fx_data.prog = 0;
 
-	const struct spa_pod *params[1];
-	uint8_t buffer[1024];
-	struct spa_pod_builder b = SPA_POD_BUILDER_INIT(buffer, sizeof(buffer));
+    const struct spa_pod* params[1];
+    uint8_t buffer[1024];
+    struct spa_pod_builder b = SPA_POD_BUILDER_INIT(buffer, sizeof(buffer));
 
-	pw_init(&argc, &argv);
+    pw_init(&argc, &argv);
 
-	data.loop = pw_main_loop_new(NULL);
+    data.loop = pw_main_loop_new(NULL);
 
-	pw_loop_add_signal(pw_main_loop_get_loop(data.loop), SIGINT, do_quit, &data);
-	pw_loop_add_signal(pw_main_loop_get_loop(data.loop), SIGTERM, do_quit, &data);
+    pw_loop_add_signal(pw_main_loop_get_loop(data.loop), SIGINT, do_quit, &data);
+    pw_loop_add_signal(pw_main_loop_get_loop(data.loop), SIGTERM, do_quit, &data);
 
-	data.filter = pw_filter_new_simple(
-			pw_main_loop_get_loop(data.loop),
-			"Guitar FX",
-			pw_properties_new(
-				PW_KEY_MEDIA_TYPE, "Audio",
-				PW_KEY_MEDIA_CATEGORY, "Filter",
-				PW_KEY_MEDIA_ROLE, "DSP",
-				NULL),
-			&filter_events,
-			&data);
+    data.filter = pw_filter_new_simple(
+        pw_main_loop_get_loop(data.loop), "Guitar FX",
+        pw_properties_new(PW_KEY_MEDIA_TYPE, "Audio", PW_KEY_MEDIA_CATEGORY,
+            "Filter", PW_KEY_MEDIA_ROLE, "DSP", NULL),
+        &filter_events, &data);
 
-	data.midi_in_port = pw_filter_add_port(data.filter,
-		    PW_DIRECTION_INPUT,
-			PW_FILTER_PORT_FLAG_MAP_BUFFERS,
-			sizeof(struct port),
-			pw_properties_new(
-			    PW_KEY_FORMAT_DSP, "8 bit raw midi",
-				PW_KEY_PORT_NAME, "midi in",
-				NULL
-			),
-			NULL, 0
-		);
+    data.midi_in_port = pw_filter_add_port(data.filter, PW_DIRECTION_INPUT,
+        PW_FILTER_PORT_FLAG_MAP_BUFFERS, sizeof(struct port),
+        pw_properties_new(PW_KEY_FORMAT_DSP, "8 bit raw midi",
+            PW_KEY_PORT_NAME, "midi in", NULL),
+        NULL, 0);
 
-	data.guit_in_port = pw_filter_add_port(data.filter,
-			PW_DIRECTION_INPUT,
-			PW_FILTER_PORT_FLAG_MAP_BUFFERS,
-			sizeof(struct port),
-			pw_properties_new(
-				PW_KEY_FORMAT_DSP, "32 bit float mono audio",
-				PW_KEY_PORT_NAME, "guitar in",
-				NULL),
-			NULL, 0);
+    data.guit_in_port = pw_filter_add_port(
+        data.filter, PW_DIRECTION_INPUT, PW_FILTER_PORT_FLAG_MAP_BUFFERS,
+        sizeof(struct port),
+        pw_properties_new(PW_KEY_FORMAT_DSP, "32 bit float mono audio",
+            PW_KEY_PORT_NAME, "guitar in", NULL),
+        NULL, 0);
 
-	data.out_port = pw_filter_add_port(data.filter,
-			PW_DIRECTION_OUTPUT,
-			PW_FILTER_PORT_FLAG_MAP_BUFFERS,
-			sizeof(struct port),
-			pw_properties_new(
-				PW_KEY_FORMAT_DSP, "32 bit float mono audio",
-				PW_KEY_PORT_NAME, "fx out",
-				NULL),
-			NULL, 0);
+    data.out_port = pw_filter_add_port(
+        data.filter, PW_DIRECTION_OUTPUT, PW_FILTER_PORT_FLAG_MAP_BUFFERS,
+        sizeof(struct port),
+        pw_properties_new(PW_KEY_FORMAT_DSP, "32 bit float mono audio",
+            PW_KEY_PORT_NAME, "fx out", NULL),
+        NULL, 0);
 
-	params[0] = spa_process_latency_build(&b,
-			SPA_PARAM_ProcessLatency,
-			&SPA_PROCESS_LATENCY_INFO_INIT(
-				.ns = 10 * SPA_NSEC_PER_MSEC
-			));
+    params[0] = spa_process_latency_build(
+        &b, SPA_PARAM_ProcessLatency,
+        &SPA_PROCESS_LATENCY_INFO_INIT(.ns = 10 * SPA_NSEC_PER_MSEC));
 
-	if (pw_filter_connect(data.filter,
-				PW_FILTER_FLAG_RT_PROCESS,
-				params, 1) < 0) {
-		fprintf(stderr, "can't connect\n");
-		return -1;
-	}
+    if (pw_filter_connect(data.filter, PW_FILTER_FLAG_RT_PROCESS, params, 1) < 0) {
+        fprintf(stderr, "can't connect\n");
+        return -1;
+    }
 
-	pw_main_loop_run(data.loop);
+    pw_main_loop_run(data.loop);
 
-	pw_filter_destroy(data.filter);
-	pw_main_loop_destroy(data.loop);
-	pw_deinit();
+    pw_filter_destroy(data.filter);
+    pw_main_loop_destroy(data.loop);
+    pw_deinit();
 
-	return 0;
+    return 0;
 }

guitfx.h

@@ -0,0 +1,16 @@
+
+#include "delay.h"
+#include "sust.h"
+#include "gate.h"
+
+struct FxData {
+    uint64_t last_tap;
+    struct delay delay;
+    struct sust sust;
+    struct gate gate;
+
+    unsigned prog;
+
+    //! elapsed time in number of samples
+    uint64_t time;
+};

meson.build

@@ -1,8 +1,15 @@
 project('guitfx', 'c')
 pipewire_dep = dependency('libpipewire-0.3')
+
+cc = meson.get_compiler('c')
+m_dep = cc.find_library('m', required : false)
+
 executable(
     'guitfx',
     'guitfx.c',
     'delay.c',
-    dependencies : [pipewire_dep],
+    'sust.c',
+    'gate.c',
+    'controller.c',
+    dependencies : [pipewire_dep, m_dep],
 )

sust.c

@@ -0,0 +1,91 @@
+#include "sust.h"
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+void sust_init(struct sust* sust)
+{
+    sust->mode = MODE_Sostenuto;
+
+    sust->playing = false;
+
+    sust->start_idx = 0;
+    sust->idx = 0;
+
+    sust->buf_len = 51200;
+    sust->record_buf = malloc(sizeof(float) * sust->buf_len);
+    sust->play_buf = malloc(sizeof(float) * sust->buf_len);
+}
+
+float envelope(float x)
+{
+    if (x < 0.4)
+        return (x / 0.4) * (x / 0.4);
+    if (x < 0.6)
+        return 1.0;
+
+    float v = 1.0 - ((x - 0.6) / 0.4);
+    return v * v;
+}
+
+float softcos(float x)
+{
+    return sin(x * (3.141 / 2.0));
+}
+
+void sust_resize(struct sust* sust, size_t new_len)
+{
+    sust->buf_len = new_len;
+    sust->play_buf = realloc(sust->play_buf, sizeof(float) * new_len);
+    sust->record_buf = realloc(sust->record_buf, sizeof(float) * new_len);
+    sust->idx %= new_len;
+}
+
+void sust_swap(
+    struct sust* sust)
+{
+    float* tmp = sust->play_buf;
+    sust->play_buf = sust->record_buf;
+    sust->record_buf = tmp;
+
+    for (int i = 0; i < sust->buf_len; ++i) {
+        sust->record_buf[i] = sust->play_buf[i];
+    }
+}
+
+void sust_process(
+    struct sust* sust,
+    size_t frame_size,
+    float const* in,
+    float* out)
+{
+    for (size_t i = 0; i < frame_size; ++i) {
+
+        if (sust->mode == MODE_Sustain && sust->idx == 0) {
+            sust_swap(sust);
+        }
+
+        float out_value = in[i];
+
+        if (sust->playing) {
+            int n_voices = 5;
+
+            for (int v = 0; v < n_voices; ++v) {
+                size_t play_idx = (sust->start_idx + sust->idx + ((v * sust->buf_len) / n_voices)) % sust->buf_len;
+                float gain = envelope(((float)play_idx) / ((float)sust->buf_len));
+                // printf("gain = %f\n", gain);
+                out_value += 0.5 * gain * sust->play_buf[play_idx];
+            }
+        }
+
+        if (sust->mode == MODE_Sostenuto) {
+            sust->record_buf[sust->idx] = in[i];
+        }
+        if (sust->mode == MODE_Sustain) {
+            sust->record_buf[sust->idx] = 0.5 * out_value;
+        }
+
+        sust->idx = (sust->idx + 1) % sust->buf_len;
+        out[i] = out_value;
+    }
+}

sust.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stddef.h>
+
+enum sust_mode {
+    MODE_Sustain = 0,
+    MODE_Sostenuto = 1
+};
+
+struct sust {
+    enum sust_mode mode;
+    bool playing;
+
+    size_t start_idx;
+    size_t idx;
+    size_t buf_len;
+    float* record_buf;
+    float* play_buf;
+};
+
+void sust_init(struct sust* sust);
+void sust_resize(struct sust* sust, size_t new_len);
+void sust_swap(struct sust* sust);
+
+void sust_process(
+    struct sust* sust,
+    size_t frame_size,
+    float const* in,
+    float* out);