mdns for arduino (esp8266)

mdns.ino

#include <ESP8266WiFi.h>
#include <WiFiClient.h>
#include <WiFiUdp.h>

#include <AppleMIDI.h>
APPLEMIDI_CREATE_DEFAULTSESSION_INSTANCE();

char ssid[] = " your ssid "; //  your network SSID (name)
char pass[] = " wifi password ";    // your network password (use for WPA, or use as key for WEP)

WiFiUDP udp;

const IPAddress tgt_ip { 224, 0, 0, 251 };
constexpr int tgt_port = 5353;

constexpr char name[] = "mymdnstest";
const int name_len = sizeof name - 1;
constexpr uint16_t port = 5004;

constexpr char applemidi[] = "_apple-midi";
constexpr int applemidi_len = sizeof applemidi - 1;
constexpr char udp_[] = "_udp";
constexpr int udp__len = sizeof udp_ - 1;
constexpr char local[] = "local";
constexpr int local_len = sizeof local - 1;

constexpr int ttl = 5;

uint8_t mdns_buffer1[256];
uint8_t mdns_buffer2[256];
char mdns_name1[64];

void flash() {
	static bool state = true;
	digitalWrite(D0, state);
	state = !state;
}

void dump(const uint8_t *const p, const uint16_t n)
{
	for(uint16_t i=0; i<n; i++) {
		if (p[i] >= 33 && p[i] < 127)
			Serial.print(' '), Serial.print((char)p[i]), Serial.print(' ');
		else
			Serial.print(p[i], HEX), Serial.print(' ');
	}

	Serial.println(F(""));
}

uint16_t add_ptr(uint8_t *const tgt)
{
	uint16_t o = 0;

	// svc name
	tgt[o++] = applemidi_len;
	o += sprintf((char *)&tgt[o], "%s", applemidi);

	tgt[o++] = udp__len;
	o += sprintf((char *)&tgt[o], "%s", udp_);

	tgt[o++] = local_len;
	o += sprintf((char *)&tgt[o], "%s", local);

	tgt[o++] = 0x00;

	tgt[o++] = 0x00;  // PTR (12)
	tgt[o++] = 0x0c;

	tgt[o++] = 0x00;  // class: in
	tgt[o++] = 0x01;

	tgt[o++] = ttl >> 24;  // ttl
	tgt[o++] = ttl >> 16;
	tgt[o++] = ttl >> 8;
	tgt[o++] = ttl;

	const uint16_t ptr_data_len = 1 + name_len +
		1 + applemidi_len +
		1 + udp__len +
		1 + local_len +
		1;
	tgt[o++] = ptr_data_len >> 8;
	tgt[o++] = ptr_data_len;

	tgt[o++] = name_len;
	o += sprintf((char *)&tgt[o], "%s", name);  // name itself

	tgt[o++] = applemidi_len;
	o += sprintf((char *)&tgt[o], "%s", applemidi);

	tgt[o++] = udp__len;
	o += sprintf((char *)&tgt[o], "%s", udp_);

	tgt[o++] = local_len;
	o += sprintf((char *)&tgt[o], "%s", local);

	tgt[o++] = 0x00;

	return o;
}

uint16_t add_srv(uint8_t *const tgt)
{
	uint16_t o = 0;

	tgt[o++] = name_len;  // length of name
	o += sprintf((char *)&tgt[o], "%s", name);  // name itself

	tgt[o++] = applemidi_len;  // length of name
	o += sprintf((char *)&tgt[o], "%s", applemidi);  // name itself

	tgt[o++] = udp__len;
	o += sprintf((char *)&tgt[o], "%s", udp_);  // name itself

	tgt[o++] = local_len;
	o += sprintf((char *)&tgt[o], "%s", local);  // name itself

	tgt[o++] = 0;  // string delimiter

	tgt[o++] = 0x00;  // type 33 SRV (server selection)
	tgt[o++] = 0x21;

	tgt[o++] = 0x80;  // class (class: cache flush, in)
	tgt[o++] = 0x01;

	tgt[o++] = ttl >> 24;  // ttl
	tgt[o++] = ttl >> 16;
	tgt[o++] = ttl >> 8;
	tgt[o++] = ttl;

	uint16_t srv_data_len = 2 + 2 + 2 + 1 + name_len + 1 + local_len + 1;
	tgt[o++] = srv_data_len >> 8;  // data len
	tgt[o++] = srv_data_len;

	tgt[o++] = 0x00;  // priority
	tgt[o++] = 0x00;

	tgt[o++] = 0x00;  // weight
	tgt[o++] = 0x00;

	tgt[o++] = port >> 8;  // port on which the apple thing listens
	tgt[o++] = port;

	tgt[o++] = name_len;  // length of name
	o += sprintf((char *)&tgt[o], "%s", name);  // name itself

	tgt[o++] = local_len;
	o += sprintf((char *)&tgt[o], "%s", local);  // name itself

	tgt[o++] = 0x00;

	return o;
}

uint16_t add_a(uint8_t *const tgt)
{
	uint16_t o = 0;

	tgt[o++] = name_len;  // length of name
	o += sprintf((char *)&tgt[o], "%s", name);  // name itself

	tgt[o++] = local_len;  // length of "local"
	o += sprintf((char *)&tgt[o], local);

	tgt[o++] = 0;  // string delimiter

	tgt[o++] = 0x00;  // type 0001 (A)
	tgt[o++] = 0x01;

	tgt[o++] = 0x80;  // class (cache flush: True, class: in)
	tgt[o++] = 0x01;

	tgt[o++] = ttl >> 24;  // ttl
	tgt[o++] = ttl >> 16;
	tgt[o++] = ttl >> 8;
	tgt[o++] = ttl;

	tgt[o++] = 0x00;  // length of address
	tgt[o++] = 0x04;

	tgt[o++] = WiFi.localIP()[0];
	tgt[o++] = WiFi.localIP()[1];
	tgt[o++] = WiFi.localIP()[2];
	tgt[o++] = WiFi.localIP()[3];

	return o;
}

void update_mdns() {
	static uint32_t last_msg = 0;
	uint32_t now = millis();

	if (last_msg == 0 || now - last_msg >= (ttl - 1) * 1000) {
		last_msg = now;

		flash();

		uint16_t ro = 0;

		mdns_buffer1[ro++] = 0x00;  // transaction id
		mdns_buffer1[ro++] = 0x00;

		mdns_buffer1[ro++] = 0x84;  // standard query response, no error
		mdns_buffer1[ro++] = 0x00;

		mdns_buffer1[ro++] = 0x00;  // 0 questions
		mdns_buffer1[ro++] = 0x00;

		mdns_buffer1[ro++] = 0x00;  // 3 answers
		mdns_buffer1[ro++] = 0x03;

		mdns_buffer1[ro++] = 0x00;  // 0 authority rr
		mdns_buffer1[ro++] = 0x00;
		
		mdns_buffer1[ro++] = 0x00;  // 0 additional rr
		mdns_buffer1[ro++] = 0x00;

		// PTR record
		ro += add_ptr(&mdns_buffer1[ro]);

		// SRV apple midi record
		ro += add_srv(&mdns_buffer1[ro]);

		// A record for the hostname to the ip-address
		ro += add_a(&mdns_buffer1[ro]);

		udp.beginPacketMulticast(tgt_ip, tgt_port, WiFi.localIP(), 2);
		udp.write(mdns_buffer1, ro);
		udp.endPacket();

		udp.beginMulticast(WiFi.localIP(), tgt_ip, 5353);
	}
}

void get_name(char *const tgt, const uint8_t *const src, uint16_t *const o_in) {
	uint16_t o_name = 0;

	tgt[0] = 0x00;

	while(src[*o_in]) {
		memcpy(&tgt[o_name], &src[*o_in + 1], src[*o_in]);
		o_name += src[*o_in];
		tgt[o_name++] = '.';
		tgt[o_name] = 0x00;

		(*o_in) += 1 + src[*o_in];
	}

	(*o_in)++; // skip 0x00
}

void check_mdns_req() {
	int n = 0;

	if ((n = udp.available()) == 0)
		return;

	if (n > sizeof mdns_buffer2)  // processing a truncated msg makes no sense
		return;

	memset(mdns_buffer2, 0x00, sizeof mdns_buffer2);
	udp.read(mdns_buffer2, sizeof mdns_buffer2);
	Serial.print(F("Recv msg, size: "));
	Serial.println(n);

	Serial.print(udp.remoteIP());
	Serial.print(' ');
	Serial.print(udp.remotePort());
	Serial.print(F(" / "));
	Serial.println(udp.destinationIP());

	dump(mdns_buffer2, n);

	uint16_t tx_id = (mdns_buffer2[0] << 8) | mdns_buffer2[1];

	uint16_t n_req = (mdns_buffer2[4] << 8) | mdns_buffer2[5];
	Serial.print(F("TX ID: "));
	Serial.print(tx_id, HEX);
	Serial.print(F(", #reqs: "));
	Serial.println(n_req);

	uint8_t n_ans = 0;

	uint16_t o_in = 12;

	memset(mdns_buffer1, 0x00, sizeof mdns_buffer1);
	uint16_t o_out = 0;
	mdns_buffer1[o_out++] = mdns_buffer2[0];  // transaction id
	mdns_buffer1[o_out++] = mdns_buffer2[1];

	mdns_buffer1[o_out++] = 0x84;  // standard query response, no error
	mdns_buffer1[o_out++] = 0x00;

	mdns_buffer1[o_out++] = 0x00;  // 0 questions
	mdns_buffer1[o_out++] = 0x00;

	uint16_t n_ans_offset = o_out;
	mdns_buffer1[o_out++] = 0x00;  // answers
	mdns_buffer1[o_out++] = 0x00;

	mdns_buffer1[o_out++] = 0x00;  // 0 authority rr
	mdns_buffer1[o_out++] = 0x00;
	
	mdns_buffer1[o_out++] = 0x00;  // 0 additional rr
	mdns_buffer1[o_out++] = 0x00;

	for(uint16_t i=0; i<n_req; i++) {
		get_name(mdns_name1, mdns_buffer2, &o_in);
		Serial.print(F("Qname: "));
		Serial.print(mdns_name1);

		uint16_t type = mdns_buffer2[o_in++] << 8;
		type += mdns_buffer2[o_in++];

		Serial.print(F(", Qtype: "));
		Serial.println(type, HEX);

		o_in += 2;  // ignore class

		bool respond = strstr(mdns_name1, applemidi) != nullptr || strstr(mdns_name1, name) != nullptr;

		Serial.println(o_out);
		if (respond) {
			if (type == 0x01)  // A
				o_out += add_a(&mdns_buffer1[o_out]), n_ans++;
			else if (type == 0x0c)  // PTR
				o_out += add_ptr(&mdns_buffer1[o_out]), n_ans++;
			else if (type == 0x21)  // SRV
				o_out += add_srv(&mdns_buffer1[o_out]), n_ans++;
		}
	}

	if (n_ans) {
		Serial.print(F("send # answers: "));
		Serial.println(n_ans);
		mdns_buffer1[n_ans_offset++] = n_ans >> 8;
		mdns_buffer1[n_ans_offset] = n_ans;

		udp.beginPacketMulticast(tgt_ip, tgt_port, WiFi.localIP(), 2);
		udp.write(mdns_buffer1, o_out);
		udp.endPacket();

		udp.beginMulticast(WiFi.localIP(), tgt_ip, 5353);

		flash();

		dump(mdns_buffer1, o_out);

		Serial.println(F(""));
	}
}

void setup() {
	Serial.begin(115200);
	Serial.println(F("Go!"));

	pinMode(D0, OUTPUT);

	WiFi.hostname(name);
	WiFi.begin(ssid, pass);

	static bool state = true;

	while (WiFi.status() != WL_CONNECTED) {
		delay(100);
		Serial.print(F("."));

		digitalWrite(D0, state);
		state = !state;
	}

	digitalWrite(D0, false);

	Serial.println(F(""));

	Serial.print("IP address:\t");
	IPAddress myIP = WiFi.localIP();
	Serial.println(myIP);

	udp.beginMulticast(WiFi.localIP(), tgt_ip, 5353);

	MIDI.begin();
}

void loop() {
	MIDI.read();

	update_mdns();

	check_mdns_req();
}