Implement OSC transmission

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 <fcntl.h>
 #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 <sys/types.h>
 #include <sys/socket.h>
 
-#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;