From 23863fc5c5c994b0044f5d6dce62a2641f07c4f5 Mon Sep 17 00:00:00 2001 From: cbdev Date: Wed, 5 Jul 2017 00:05:13 +0200 Subject: Implement OSC transmission --- osc.c | 286 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++--- osc.h | 7 +- 2 files changed, 279 insertions(+), 14 deletions(-) diff --git a/osc.c b/osc.c index 660c9ac..7b1115a 100644 --- a/osc.c +++ b/osc.c @@ -6,6 +6,12 @@ #include #include "osc.h" +/* + * TODO + * dest = learn for learning client addresses + * ping method + */ + #define osc_align(a) ((((a) / 4) + (((a) % 4) ? 1 : 0)) * 4) #define BACKEND_NAME "osc" @@ -83,12 +89,54 @@ static inline osc_parameter_value osc_parse(osc_parameter_type t, uint8_t* data) return v; } -static inline channel_value osc_parameter_interpolate(osc_parameter_type t, osc_parameter_value min, osc_parameter_value max, osc_parameter_value cur){ +static inline int osc_deparse(osc_parameter_type t, osc_parameter_value v, uint8_t* data){ + uint64_t u64 = 0; + uint32_t u32 = 0; + switch(t){ + case int32: + case float32: + u32 = htobe32(v.i32); + memcpy(data, &u32, sizeof(u32)); + break; + case int64: + case double64: + u64 = htobe64(v.i64); + memcpy(data, &u64, sizeof(u64)); + break; + default: + fprintf(stderr, "Invalid OSC type passed to parsing routine\n"); + return 1; + } + return 0; +} + +static inline osc_parameter_value osc_parse_value_spec(osc_parameter_type t, char* value){ + osc_parameter_value v = {0}; + switch(t){ + case int32: + v.i32 = strtol(value, NULL, 0); + break; + case float32: + v.f = strtof(value, NULL); + break; + case int64: + v.i64 = strtoll(value, NULL, 0); + break; + case double64: + v.d = strtod(value, NULL); + break; + default: + fprintf(stderr, "Invalid OSC type passed to value parser\n"); + } + return v; +} + +static inline channel_value osc_parameter_normalise(osc_parameter_type t, osc_parameter_value min, osc_parameter_value max, osc_parameter_value cur){ channel_value v = { .raw = {0}, .normalised = 0 }; - + union { uint32_t u32; float f32; @@ -132,6 +180,40 @@ static inline channel_value osc_parameter_interpolate(osc_parameter_type t, osc_ return v; } +static inline osc_parameter_value osc_parameter_denormalise(osc_parameter_type t, osc_parameter_value min, osc_parameter_value max, channel_value cur){ + osc_parameter_value v = {0}; + + union { + uint32_t u32; + float f32; + uint64_t u64; + double d64; + } range; + + switch(t){ + case int32: + range.u32 = max.i32 - min.i32; + v.i32 = (range.u32 * cur.normalised) + min.i32; + break; + case float32: + range.f32 = max.f - min.f; + v.f = (range.f32 * cur.normalised) + min.f; + break; + case int64: + range.u64 = max.i64 - min.i64; + v.i64 = (range.u64 * cur.normalised) + min.i64; + break; + case double64: + range.d64 = max.d - min.d; + v.d = (range.d64 * cur.normalised) + min.d; + break; + default: + fprintf(stderr, "Invalid OSC type passed to interpolation routine\n"); + } + + return v; +} + static int osc_generate_event(channel* c, osc_channel* info, char* fmt, uint8_t* data, size_t data_len){ size_t p, off = 0; if(!c || !info){ @@ -160,7 +242,7 @@ static int osc_generate_event(channel* c, osc_channel* info, char* fmt, uint8_t* } cur = osc_parse(fmt[info->param_index], data + off); - evt = osc_parameter_interpolate(fmt[info->param_index], min, max, cur); + evt = osc_parameter_normalise(fmt[info->param_index], min, max, cur); return mm_channel_event(c, evt); } @@ -287,10 +369,12 @@ static int backend_configure(char* option, char* value){ static int backend_configure_instance(instance* inst, char* option, char* value){ osc_instance* data = (osc_instance*) inst->impl; - char* host = NULL, *port = NULL; + char* host = NULL, *port = NULL, *token = NULL, *format = NULL; + size_t u, p; if(!strcmp(option, "root")){ if(osc_validate_path(value)){ + fprintf(stderr, "Not a valid OSC root: %s\n", value); return 1; } @@ -319,6 +403,16 @@ static int backend_configure_instance(instance* inst, char* option, char* value) return 0; } else if(!strcmp(option, "dest")){ + if(!strncmp(value, "learn", 5)){ + data->learn = 1; + + //check if a forced port was provided + if(value[5] == '@'){ + data->forced_rport = strtoul(value + 6, NULL, 0); + } + return 0; + } + if(osc_separate_hostspec(value, &host, &port)){ fprintf(stderr, "Invalid destination address for instance %s\n", inst->name); return 1; @@ -330,9 +424,79 @@ static int backend_configure_instance(instance* inst, char* option, char* value) } return 0; } + else if(*option == '/'){ + //pre-configure channel + if(osc_validate_path(option)){ + fprintf(stderr, "Not a valid OSC path: %s\n", option); + return 1; + } + + for(u = 0; u < data->channels; u++){ + if(!strcmp(option, data->channel[u].path)){ + fprintf(stderr, "OSC channel %s already configured\n", option); + return 1; + } + } + + //tokenize configuration + format = strtok(value, " "); + if(!format || strlen(format) < 1){ + fprintf(stderr, "Not a valid format for OSC path %s\n", option); + return 1; + } + + //check format validity, create subchannels + for(p = 0; p < strlen(format); p++){ + if(!osc_data_length(format[p])){ + fprintf(stderr, "Invalid format specifier %c for path %s, ignoring\n", format[p], option); + continue; + } + + //register new sub-channel + data->channel = realloc(data->channel, (data->channels + 1) * sizeof(osc_channel)); + if(!data->channel){ + fprintf(stderr, "Failed to allocate memory\n"); + return 1; + } + + memset(data->channel + data->channels, 0, sizeof(osc_channel)); + data->channel[data->channels].params = strlen(format); + data->channel[data->channels].param_index = p; + data->channel[data->channels].type = format[p]; + data->channel[data->channels].path = strdup(option); + + if(!data->channel[data->channels].path){ + fprintf(stderr, "Failed to allocate memory\n"); + return 1; + } + + //parse min/max values + token = strtok(NULL, " "); + if(!token){ + fprintf(stderr, "Missing minimum specification for parameter %zu of %s\n", p, option); + return 1; + } + data->channel[data->channels].min = osc_parse_value_spec(format[p], token); + + token = strtok(NULL, " "); + if(!token){ + fprintf(stderr, "Missing maximum specification for parameter %zu of %s\n", p, option); + return 1; + } + data->channel[data->channels].max = osc_parse_value_spec(format[p], token); + + //allocate channel from core + if(!mm_channel(inst, data->channels, 1)){ + fprintf(stderr, "Failed to register core channel\n"); + return 1; + } + + //increase channel count + data->channels++; + } + return 0; + } - //TODO channel configuration - //create path.INDEX channels fprintf(stderr, "Unknown configuration parameter %s for OSC backend\n", option); return 1; } @@ -370,6 +534,7 @@ static channel* backend_channel(instance* inst, char* spec){ //find matching channel for(u = 0; u < data->channels; u++){ if(!strcmp(spec, data->channel[u].path) && data->channel[u].param_index == param_index){ + //fprintf(stderr, "Reusing previously created channel %s parameter %zu\n", data->channel[u].path, data->channel[u].param_index); break; } } @@ -397,13 +562,107 @@ static channel* backend_channel(instance* inst, char* spec){ } static int backend_set(instance* inst, size_t num, channel** c, channel_value* v){ + uint8_t xmit_buf[OSC_XMIT_BUF], *format = NULL; + size_t evt = 0, off, members, p; + if(!num){ + return 0; + } + + memset(xmit_buf, 0, sizeof(xmit_buf)); osc_instance* data = (osc_instance*) inst->impl; if(!data->dest_len){ - fprintf(stderr, "OSC instance %s does not have a destination, output is disabled\n", inst->name); + fprintf(stderr, "OSC instance %s does not have a destination, output is disabled (%zu channels)\n", inst->name, num); return 0; } - //TODO aggregate dot-channels + for(evt = 0; evt < num; evt++){ + off = c[evt]->ident; + + //sanity check + if(off >= data->channels){ + fprintf(stderr, "OSC channel identifier out of range\n"); + return 1; + } + + //if the format is unknown, don't output + if(data->channel[off].type == not_set || data->channel[off].params == 0){ + fprintf(stderr, "OSC channel %s.%s requires format specification for output\n", inst->name, data->channel[off].path); + continue; + } + + //update current value + data->channel[off].current = osc_parameter_denormalise(data->channel[off].type, data->channel[off].min, data->channel[off].max, v[evt]); + //mark channel + data->channel[off].mark = 1; + } + + //fix destination rport if required + if(data->forced_rport){ + //cheating a bit because both IPv4 and IPv6 have the port at the same offset + struct sockaddr_in* sockadd = (struct sockaddr_in*) &(data->dest); + sockadd->sin_port = htobe16(data->forced_rport); + } + + //find all marked channels + for(evt = 0; evt < data->channels; evt++){ + if(data->channel[evt].mark){ + //determine minimum packet size + if(osc_align((data->root ? strlen(data->root) : 0) + strlen(data->channel[evt].path) + 1) + osc_align(data->channel[evt].params + 2) >= sizeof(xmit_buf)){ + fprintf(stderr, "Insufficient buffer size for OSC transmitting channel %s.%s\n", inst->name, data->channel[evt].path); + return 1; + } + + off = 0; + //copy osc target path + if(data->root){ + memcpy(xmit_buf, data->root, strlen(data->root)); + off += strlen(data->root); + } + memcpy(xmit_buf + off, data->channel[evt].path, strlen(data->channel[evt].path)); + off += strlen(data->channel[evt].path) + 1; + off = osc_align(off); + + //get format string offset, initialize + format = xmit_buf + off; + off += osc_align(data->channel[evt].params + 2); + *format = ','; + format++; + + //gather subchannels, unmark + members = 0; + for(p = 0; p < data->channels && members < data->channel[evt].params; p++){ + if(!strcmp(data->channel[evt].path, data->channel[p].path)){ + //unmark channel + data->channel[p].mark = 0; + + //sanity check + if(data->channel[p].param_index >= data->channel[evt].params){ + fprintf(stderr, "OSC channel %s.%s has multiple parameter offset definitions\n", inst->name, data->channel[evt].path); + return 1; + } + + //write format specifier + format[data->channel[p].param_index] = data->channel[p].type; + + //write data + //FIXME this currently depends on all channels being registered in the correct order, since it just appends data + if(off + osc_data_length(data->channel[p].type) >= sizeof(xmit_buf)){ + fprintf(stderr, "Insufficient buffer size for OSC transmitting channel %s.%s at parameter %zu\n", inst->name, data->channel[evt].path, members); + return 1; + } + + osc_deparse(data->channel[p].type, data->channel[p].current, xmit_buf + off); + off += osc_data_length(data->channel[p].type); + members++; + } + } + + //output packet + if(sendto(data->fd, xmit_buf, off, 0, (struct sockaddr*) &(data->dest), data->dest_len) < 0){ + fprintf(stderr, "Failed to transmit OSC packet: %s\n", strerror(errno)); + } + } + } return 0; } @@ -428,7 +687,13 @@ static int backend_handle(size_t num, managed_fd* fds){ data = (osc_instance*) inst->impl; do{ - bytes_read = recv(fds[fd].fd, recv_buf, sizeof(recv_buf), 0); + if(data->learn){ + data->dest_len = sizeof(data->dest); + bytes_read = recvfrom(fds[fd].fd, recv_buf, sizeof(recv_buf), 0, (struct sockaddr*) &(data->dest), &(data->dest_len)); + } + else{ + bytes_read = recv(fds[fd].fd, recv_buf, sizeof(recv_buf), 0); + } if(data->root && strncmp(recv_buf, data->root, strlen(data->root))){ //ignore packet for different root continue; @@ -453,7 +718,7 @@ static int backend_handle(size_t num, managed_fd* fds){ for(c = 0; c < data->channels; c++){ //FIXME implement proper OSC path match //prefix match - if(!strncmp(osc_local, data->channel[c].path, strlen(data->channel[c].path)) && strlen(data->channel[c].path) == strlen(osc_local)){ + if(!strcmp(osc_local, data->channel[c].path)){ if(strlen(osc_fmt) > data->channel[c].param_index){ //fprintf(stderr, "Taking parameter %zu of %s (%s), %zd bytes, data offset %zu\n", data->channel[c].param_index, recv_buf, osc_fmt, bytes_read, (osc_data - (uint8_t*)recv_buf)); if(osc_generate_event(mm_channel(inst, c, 0), data->channel + c, osc_fmt, osc_data, bytes_read - (osc_data - (uint8_t*) recv_buf))){ @@ -531,7 +796,6 @@ static int backend_shutdown(){ data = (osc_instance*) inst[u]->impl; for(c = 0; c < data->channels; c++){ free(data->channel[c].path); - free(data->channel[c].param); } free(data->channel); free(data->root); diff --git a/osc.h b/osc.h index 8fcad71..5938f12 100644 --- a/osc.h +++ b/osc.h @@ -2,7 +2,8 @@ #include #include -#define OSC_RECV_BUF 4096 +#define OSC_RECV_BUF 8192 +#define OSC_XMIT_BUF 8192 int init(); static int backend_configure(char* option, char* value); @@ -34,7 +35,6 @@ typedef struct /*_osc_channel*/ { char* path; size_t params; size_t param_index; - size_t* param; uint8_t mark; osc_parameter_type type; @@ -46,9 +46,10 @@ typedef struct /*_osc_channel*/ { typedef struct /*_osc_instance_data*/ { size_t channels; osc_channel* channel; - uint8_t output; char* root; socklen_t dest_len; struct sockaddr_storage dest; int fd; + uint8_t learn; + uint16_t forced_rport; } osc_instance; -- cgit v1.2.3