1 files changed, 183 insertions, 31 deletions

diff --git a/backends/rtpmidi.c b/backends/rtpmidi.c
index 7c5aa69..d349e6f 100644
--- a/backends/rtpmidi.c
+++ b/backends/rtpmidi.c
@@ -427,6 +427,12 @@ static char* rtpmidi_type_name(uint8_t type){
 			return "aftertouch";
 		case pitchbend:
 			return "pitch";
+		case program:
+			return "program";
+		case rpn:
+			return "rpn";
+		case nrpn:
+			return "nrpn";
 	}
 	return "unknown";
 }
@@ -552,7 +558,7 @@ static int rtpmidi_peer_applecommand(instance* inst, size_t peer, uint8_t contro
 
 	memcpy(&dest_addr, &(data->peer[peer].dest), min(sizeof(dest_addr), data->peer[peer].dest_len));
 	if(control){
-	//calculate remote control port from data port
+		//calculate remote control port from data port
 		((struct sockaddr_in*) &dest_addr)->sin_port = htobe16(be16toh(((struct sockaddr_in*) &dest_addr)->sin_port) - 1);
 	}
 
@@ -577,6 +583,13 @@ static int rtpmidi_configure_instance(instance* inst, char* option, char* value)
 		LOGPF("Unknown instance mode %s for instance %s", value, inst->name);
 		return 1;
 	}
+	else if(!strcmp(option, "epn-tx")){
+		data->epn_tx_short = 0;
+		if(!strcmp(value, "short")){
+			data->epn_tx_short = 1;
+		}
+		return 0;
+	}
 	else if(!strcmp(option, "ssrc")){
 		data->ssrc = strtoul(value, NULL, 0);
 		if(!data->ssrc){
@@ -705,6 +718,14 @@ static channel* rtpmidi_channel(instance* inst, char* spec, uint8_t flags){
 		ident.fields.type = note;
 		next_token += 4;
 	}
+	else if(!strncmp(next_token, "rpn", 3)){
+		ident.fields.type = rpn;
+		next_token += 3;
+	}
+	else if(!strncmp(next_token, "nrpn", 4)){
+		ident.fields.type = nrpn;
+		next_token += 4;
+	}
 	else if(!strncmp(next_token, "pressure", 8)){
 		ident.fields.type = pressure;
 		next_token += 8;
@@ -715,6 +736,9 @@ static channel* rtpmidi_channel(instance* inst, char* spec, uint8_t flags){
 	else if(!strncmp(next_token, "aftertouch", 10)){
 		ident.fields.type = aftertouch;
 	}
+	else if(!strncmp(next_token, "program", 7)){
+		ident.fields.type = program;
+	}
 	else{
 		LOGPF("Unknown control type in spec %s", spec);
 		return NULL;
@@ -728,12 +752,38 @@ static channel* rtpmidi_channel(instance* inst, char* spec, uint8_t flags){
 	return NULL;
 }
 
+static size_t rtpmidi_push_midi(uint8_t* payload, size_t bytes_left, uint8_t type, uint8_t channel, uint8_t control, uint16_t value){
+	//FIXME this is a bit simplistic but it works for now
+	if(bytes_left < 4){
+		return 0;
+	}
+
+	//encode timestamp
+	payload[0] = 0;
+
+	//encode midi command
+	payload[1] = type | channel;
+	payload[2] = control;
+	payload[3] = value & 0x7F;
+
+	if(type == pitchbend){
+		payload[2] = value & 0x7F;
+		payload[3] = (value >> 7) & 0x7F;
+	}
+	//channel-wides aftertouch and program are only 2 bytes
+	else if(type == aftertouch || type == program){
+		payload[2] = payload[3];
+		return 3;
+	}
+	return 4;
+}
+
 static int rtpmidi_set(instance* inst, size_t num, channel** c, channel_value* v){
 	rtpmidi_instance_data* data = (rtpmidi_instance_data*) inst->impl;
 	uint8_t frame[RTPMIDI_PACKET_BUFFER] = "";
 	rtpmidi_header* rtp_header = (rtpmidi_header*) frame;
 	rtpmidi_command_header* command_header = (rtpmidi_command_header*) (frame + sizeof(rtpmidi_header));
-	size_t offset = sizeof(rtpmidi_header) + sizeof(rtpmidi_command_header), u = 0;
+	size_t command_length = 0, offset = sizeof(rtpmidi_header) + sizeof(rtpmidi_command_header), u = 0;
 	uint8_t* payload = frame + offset;
 	rtpmidi_channel_ident ident;
 
@@ -752,27 +802,37 @@ static int rtpmidi_set(instance* inst, size_t num, channel** c, channel_value* v
 	for(u = 0; u < num; u++){
 		ident.label = c[u]->ident;
 
-		//encode timestamp
-		payload[0] = 0;
-
-		//encode midi command
-		payload[1] = ident.fields.type | ident.fields.channel;
-		payload[2] = ident.fields.control;
-		payload[3] = v[u].normalised * 127.0;
-
-		if(ident.fields.type == pitchbend){
-			payload[2] = ((int)(v[u].normalised * 16384.0)) & 0x7F;
-			payload[3] = (((int)(v[u].normalised * 16384.0)) >> 7) & 0x7F;
+		switch(ident.fields.type){
+			case rpn:
+			case nrpn:
+				//transmit parameter number
+				command_length = rtpmidi_push_midi(payload + offset, sizeof(frame) - offset, cc, ident.fields.channel, (ident.fields.type == rpn) ? 101 : 99, (ident.fields.control >> 7) & 0x7F);
+				command_length += rtpmidi_push_midi(payload + offset + command_length, sizeof(frame) - offset, cc, ident.fields.channel, (ident.fields.type == rpn) ? 100 : 98, ident.fields.control & 0x7F);
+
+				//transmit parameter value
+				command_length += rtpmidi_push_midi(payload + offset + command_length, sizeof(frame) - offset, cc, ident.fields.channel, 6, (((uint16_t) (v[u].normalised * 16383.0)) >> 7) & 0x7F);
+				command_length += rtpmidi_push_midi(payload + offset + command_length, sizeof(frame) - offset, cc, ident.fields.channel, 38, ((uint16_t) (v[u].normalised * 16383.0)) & 0x7F);
+
+				if(!data->epn_tx_short){
+					//clear active parameter
+					command_length += rtpmidi_push_midi(payload + offset + command_length, sizeof(frame) - offset, cc, ident.fields.channel, 101, 127);
+					command_length += rtpmidi_push_midi(payload + offset + command_length, sizeof(frame) - offset, cc, ident.fields.channel, 100, 127);
+				}
+				break;
+			case pitchbend:
+				//TODO check whether this works
+				command_length = rtpmidi_push_midi(payload + offset, sizeof(frame) - offset, ident.fields.type, ident.fields.channel, ident.fields.control, v[u].normalised * 16383.0);
+				break;
+			default:
+				command_length = rtpmidi_push_midi(payload + offset, sizeof(frame) - offset, ident.fields.type, ident.fields.channel, ident.fields.control, v[u].normalised * 127.0);
 		}
-		//channel-wide aftertouch is only 2 bytes
-		else if(ident.fields.type == aftertouch){
-			payload[2] = payload[3];
-			payload -= 1;
-			offset -= 1;
+
+		if(command_length == 0){
+			LOGPF("Transmit buffer size exceeded on %s", inst->name);
+			break;
 		}
 
-		payload += 4;
-		offset += 4;
+		offset += command_length;
 	}
 
 	//update command section length
@@ -784,7 +844,9 @@ static int rtpmidi_set(instance* inst, size_t num, channel** c, channel_value* v
 
 	for(u = 0; u < data->peers; u++){
 		if(data->peer[u].active && data->peer[u].connected){
-			sendto(data->fd, frame, offset, 0, (struct sockaddr*) &data->peer[u].dest, data->peer[u].dest_len);
+			if(sendto(data->fd, frame, offset, 0, (struct sockaddr*) &data->peer[u].dest, data->peer[u].dest_len) <= 0){
+				LOGPF("Failed to transmit to peer: %s", mmbackend_socket_strerror(errno));
+			}
 		}
 	}
 
@@ -924,6 +986,79 @@ static int rtpmidi_handle_applemidi(instance* inst, int fd, uint8_t* frame, size
 	return 0;
 }
 
+//this state machine was copied more-or-less verbatim from the alsa midi implementation - fixes there will need to be integrated
+static void rtpmidi_handle_epn(instance* inst, uint8_t chan, uint16_t control, uint16_t value){
+	rtpmidi_instance_data* data = (rtpmidi_instance_data*) inst->impl;
+	rtpmidi_channel_ident ident = {
+		.label = 0
+	};
+	channel* changed = NULL;
+	channel_value val;
+
+	//switching between nrpn and rpn clears all valid bits
+	if(((data->epn_status[chan] & EPN_NRPN) && (control == 101 || control == 100))
+				|| (!(data->epn_status[chan] & EPN_NRPN) && (control == 99 || control == 98))){
+		data->epn_status[chan] &= ~(EPN_NRPN | EPN_PARAMETER_LO | EPN_PARAMETER_HI);
+	}
+
+	//setting an address always invalidates the value valid bits
+	if(control >= 98 && control <= 101){
+		data->epn_status[chan] &= ~EPN_VALUE_HI;
+	}
+
+	//parameter hi
+	if(control == 101 || control == 99){
+		data->epn_control[chan] &= 0x7F;
+		data->epn_control[chan] |= value << 7;
+		data->epn_status[chan] |= EPN_PARAMETER_HI | ((control == 99) ? EPN_NRPN : 0);
+		if(control == 101 && value == 127){
+			data->epn_status[chan] &= ~EPN_PARAMETER_HI;
+		}
+	}
+
+	//parameter lo
+	if(control == 100 || control == 98){
+		data->epn_control[chan] &= ~0x7F;
+		data->epn_control[chan] |= value & 0x7F;
+		data->epn_status[chan] |= EPN_PARAMETER_LO | ((control == 98) ? EPN_NRPN : 0);
+		if(control == 100 && value == 127){
+			data->epn_status[chan] &= ~EPN_PARAMETER_LO;
+		}
+	}
+
+	//value hi, clears low, mark as update candidate
+	if(control == 6
+			//check if parameter is set before accepting value update
+			&& ((data->epn_status[chan] & (EPN_PARAMETER_HI | EPN_PARAMETER_LO)) == (EPN_PARAMETER_HI | EPN_PARAMETER_LO))){
+		data->epn_value[chan] = value << 7;
+		data->epn_status[chan] |= EPN_VALUE_HI;
+	}
+
+	//value lo, flush the value
+	if(control == 38
+			&& data->epn_status[chan] & EPN_VALUE_HI){
+		data->epn_value[chan] &= ~0x7F;
+		data->epn_value[chan] |= value & 0x7F;
+		data->epn_status[chan] &= ~EPN_VALUE_HI;
+
+		if(cfg.detect){
+			LOGPF("Incoming EPN data on channel %s.ch%d.%s%d", inst->name, chan, data->epn_status[chan] & EPN_NRPN ? "nrpn" : "rpn", data->epn_control[chan]);
+		}
+
+		//find the updated channel
+		ident.fields.type = data->epn_status[chan] & EPN_NRPN ? nrpn : rpn;
+		ident.fields.channel = chan;
+		ident.fields.control = data->epn_control[chan];
+		val.normalised = (double) data->epn_value[chan] / 16383.0;
+
+		//push the new value
+		changed = mm_channel(inst, ident.label, 0);
+		if(changed){
+			mm_channel_event(changed, val);
+		}
+	}
+}
+
 static int rtpmidi_parse(instance* inst, uint8_t* frame, size_t bytes){
 	uint16_t length = 0;
 	size_t offset = 1, decode_time = 0, command_bytes = 0;
@@ -996,9 +1131,10 @@ static int rtpmidi_parse(instance* inst, uint8_t* frame, size_t bytes){
 		ident.fields.channel = midi_status & 0x0F;
 
 		//single byte command
-		if(ident.fields.type == aftertouch){
+		if(ident.fields.type == aftertouch || ident.fields.type == program){
 			ident.fields.control = 0;
 			val.normalised = (double) frame[offset] / 127.0;
+			val.raw.u64 = frame[offset];
 			offset++;
 		}
 		//two-byte command
@@ -1010,17 +1146,20 @@ static int rtpmidi_parse(instance* inst, uint8_t* frame, size_t bytes){
 
 			if(ident.fields.type == pitchbend){
 				ident.fields.control = 0;
-				val.normalised = (double)((frame[offset] << 7) | frame[offset - 1]) / 16384.0;
+				val.normalised = (double)((frame[offset] << 7) | frame[offset - 1]) / 16383.0;
+				val.raw.u64 = (frame[offset] << 7) | frame[offset - 1];
 			}
 			else{
 				ident.fields.control = frame[offset - 1];
 				val.normalised = (double) frame[offset] / 127.0;
+				val.raw.u64 = frame[offset];
 			}
 
 			//fix-up note off events
 			if(ident.fields.type == 0x80){
 				ident.fields.type = note;
 				val.normalised = 0;
+				val.raw.u64 = 0;
 			}
 
 			offset++;
@@ -1029,8 +1168,18 @@ static int rtpmidi_parse(instance* inst, uint8_t* frame, size_t bytes){
 		DBGPF("Decoded command type %02X channel %d control %d value %f",
 				ident.fields.type, ident.fields.channel, ident.fields.control, val.normalised);
 
+		//forward EPN CCs to the EPN state machine
+		if(ident.fields.type == cc
+				&& ((ident.fields.control <= 101 && ident.fields.control >= 98)
+					|| ident.fields.control == 6
+					|| ident.fields.control == 38)){
+			rtpmidi_handle_epn(inst, ident.fields.channel, ident.fields.control, val.raw.u64);
+		}	
+
 		if(cfg.detect){
-			if(ident.fields.type == pitchbend || ident.fields.type == aftertouch){
+			if(ident.fields.type == pitchbend
+					|| ident.fields.type == aftertouch
+					|| ident.fields.type == program){
 				LOGPF("Incoming data on channel %s.ch%d.%s, value %f",
 						inst->name, ident.fields.channel,
 						rtpmidi_type_name(ident.fields.type), val.normalised);
@@ -1150,8 +1299,10 @@ static int rtpmidi_mdns_broadcast(uint8_t* frame, size_t len){
 	};
 
 	//send to ipv4 and ipv6 mcasts
-	sendto(cfg.mdns_fd, frame, len, 0, (struct sockaddr*) &mcast6, sizeof(mcast6));
-	sendto(cfg.mdns4_fd, frame, len, 0, (struct sockaddr*) &mcast, sizeof(mcast));
+	if((sendto(cfg.mdns_fd, frame, len, 0, (struct sockaddr*) &mcast6, sizeof(mcast6)) != len)
+			| (sendto(cfg.mdns4_fd, frame, len, 0, (struct sockaddr*) &mcast, sizeof(mcast)) != len)){
+		LOG("Failed to transmit mDNS frame");
+	}
 	return 0;
 }
 
@@ -1180,13 +1331,14 @@ static int rtpmidi_mdns_detach(instance* inst){
 	}
 	offset += bytes;
 
-	//TODO length-checks here
-	frame[offset++] = strlen(inst->name);
-	memcpy(frame + offset, inst->name, strlen(inst->name));
-	offset += strlen(inst->name);
+	//calculate maximum permitted instance name length
+	bytes = min(min(strlen(inst->name), sizeof(frame) - offset - 3), 255);
+	frame[offset++] = bytes;
+	memcpy(frame + offset, inst->name, bytes);
+	offset += bytes;
 	frame[offset++] = 0xC0;
 	frame[offset++] = sizeof(dns_header);
-	rr->data = htobe16(1 + strlen(inst->name) + 2);
+	rr->data = htobe16(1 + bytes + 2);
 
 	free(name.name);
 	return rtpmidi_mdns_broadcast(frame, offset);