~hp/chimaera_firmware

fdf3e30a24dc107d17d1440e031a9c0fc413a734 — Hanspeter Portner 2 years ago 184071c
remove superfluous stuff.

* arp
* dhcpc
* mdns-sd
* sntp
* ptp
* tuio1
* scsynth
* dummy
* custom
* ipv4ll
40 files changed, 13 insertions(+), 6199 deletions(-)

M VERSION
D arp/arp.c
D arp/arp_private.h
M chimutil/chimutil.c
M cmc/cmc.c
M config/config.c
D custom/custom.c
D custom/custom_private.h
D custom/custom_rpn.c
D dhcpc/dhcpc.c
D dhcpc/dhcpc_private.h
D dummy/dummy.c
D engines/custom.h
D engines/dummy.h
D engines/scsynth.h
D engines/tuio1.h
M firmware.c
D include/arp.h
M include/chimaera.h
M include/chimutil.h
M include/config.h
D include/dhcpc.h
M include/engines.h
D include/ipv4ll.h
D include/mdns-sd.h
D include/ptp.h
M include/sensors.h
D include/sntp.h
D ipv4ll/ipv4ll.c
D mdns-sd/mdns-sd.c
D mdns-sd/mdns-sd_private.h
D ptp/ptp.c
D ptp/ptp_private.h
M rules.mk
D scsynth/scsynth.c
M sensors/sensors.c
D sntp/sntp.c
D sntp/sntp_private.h
D tuio1/tuio1.c
M tuio2/tuio2.c
M VERSION => VERSION +1 -1
@@ 1,1 1,1 @@
0.15.3
0.15.7

D arp/arp.c => arp/arp.c +0 -202
@@ 1,202 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#include "arp_private.h"

#include <string.h>
#include <stdio.h>

#include <wiz.h>
#include <config.h>
#include <chimaera.h>
#include <sntp.h>

#include <libmaple/systick.h>

static MACRAW_Header header;
static ARP_Payload payload;
static volatile uint_fast8_t arp_collision = 0;

/*
 * fill macraw packet with ARP payload
 */
static void
_arp_fill(uint16_t oper, uint8_t *src_mac, uint8_t *src_ip, uint8_t *tar_mac, uint8_t *tar_ip)
{
	// fill MACRAW header
	memcpy(header.dst_mac, tar_mac, 6);
	memcpy(header.src_mac, src_mac, 6);
	header.type = hton(MACRAW_TYPE_ARP);

	// fill ARP payload
	payload.htype = hton(ARP_HTYPE_ETHERNET);
	payload.ptype = hton(ARP_PTYPE_IPV4);
	payload.hlen = ARP_HLEN_ETHERNET;
	payload.plen = ARP_PLEN_IPV4;
	payload.oper = hton(oper);

	memcpy(payload.sha, src_mac, 6);
	memcpy(payload.spa, src_ip, 4);
	memcpy(payload.tha, tar_mac, 6);
	memcpy(payload.tpa, tar_ip, 4);
}

/*
 * send and ARP probe with mac and ip
 */
static void
_arp_fill_probe(uint8_t *mac, uint8_t *ip)
{
	_arp_fill(ARP_OPER_REQUEST, mac,(uint8_t *)wiz_nil_ip,(uint8_t *)wiz_broadcast_mac, ip);
}

/*
 * send and ARP announce with mac and ip
 */
static void
_arp_fill_announce(uint8_t *mac, uint8_t *ip)
{
	_arp_fill(ARP_OPER_REQUEST, mac, ip,(uint8_t *)wiz_broadcast_mac, ip);
}

/*
 * callback to handle replies to the ARP probes and announces
 */
static void
arp_reply_cb(uint8_t *buf, uint16_t len, void *data)
{
	(void)len;
	uint8_t *ip = data;

	MACRAW_Header *mac_header =(MACRAW_Header *)buf;

	// check whether this is an ethernet ARP packet
	if(hton(mac_header->type) != MACRAW_TYPE_ARP)
		return;

	ARP_Payload *arp_reply =(ARP_Payload *)(buf + MACRAW_HEADER_SIZE);

	// check whether this is an IPv4 ARP reply
	if( (hton(arp_reply->htype) != ARP_HTYPE_ETHERNET) ||
			(hton(arp_reply->ptype) != ARP_PTYPE_IPV4) ||
			(arp_reply->hlen != ARP_HLEN_ETHERNET) ||
			(arp_reply->plen != ARP_PLEN_IPV4) ||
			(hton(arp_reply->oper) != ARP_OPER_REPLY) )
		return;

	// check whether the ARP is for us
	if(memcmp(arp_reply->tha, config.comm.mac, 6))
		return;

	// check whether the ARP has our requested IP as source -> then there is an ARP probe collision
	if(!memcmp(arp_reply->spa, ip, 4))
		arp_collision = 1;
}

/*
 * return number of systicks representing a random delay in seconds in the range [minsecs:maxsecs]
 */
static uint32_t
_random_ticks(uint32_t minsecs, uint32_t maxsecs)
{
	uint32_t span = maxsecs - minsecs;
	//return SNTP_SYSTICK_RATE*(minsecs +(float)rand() /(RAND_MAX / span));
	return SNTP_SYSTICK_RATE*(minsecs +(float)rand() / RAND_MAX * span);
}

uint_fast8_t
arp_probe(uint8_t sock, uint8_t *ip)
{
	uint32_t tick;
	uint32_t arp_timeout;

	// open socket in MACRAW mode
	macraw_begin(sock, 1);

	_arp_fill_probe(config.comm.mac, ip);

	// initial delay
	tick = systick_uptime();
	arp_timeout = _random_ticks(0, ARP_PROBE_WAIT);
	while(systick_uptime() - tick < arp_timeout)
		; // wait for random delay

	arp_collision = 0;
	uint_fast8_t i;
	for(i=ARP_PROBE_NUM; i>0; i--)
	{
		// serialize MACRAW packet with ARP payload
		memcpy(&BUF_O_BASE(buf_o_ptr)[WIZ_SEND_OFFSET], &header, MACRAW_HEADER_SIZE);
		memcpy(&BUF_O_BASE(buf_o_ptr)[WIZ_SEND_OFFSET + MACRAW_HEADER_SIZE], &payload, ARP_PAYLOAD_SIZE);
		// send packet
		macraw_send(sock, BUF_O_BASE(buf_o_ptr), MACRAW_HEADER_SIZE + ARP_PAYLOAD_SIZE);
		
		// delay between probes
		if(i>1)
		{
			tick = systick_uptime();
			arp_timeout = _random_ticks(ARP_PROBE_MIN, ARP_PROBE_MAX);
			while(!arp_collision && (systick_uptime() - tick < arp_timeout) )
				macraw_dispatch(sock, BUF_O_BASE(buf_o_ptr), arp_reply_cb, ip);
		}
	}

	// delay before first announce
	tick = systick_uptime();
	arp_timeout = SNTP_SYSTICK_RATE*ARP_ANNOUNCE_WAIT;
	while(!arp_collision && (systick_uptime() - tick < arp_timeout) )
		macraw_dispatch(sock, BUF_O_BASE(buf_o_ptr), arp_reply_cb, ip);

	// close MACRAW socket
	macraw_end(sock);

	return arp_collision;
}

void
arp_announce(uint8_t sock, uint8_t *ip)
{
	uint32_t tick;
	uint32_t arp_timeout;

	// open socket in MACRAW mode
	macraw_begin(sock, 1);

	_arp_fill_announce(config.comm.mac, ip);

	uint_fast8_t i;
	for(i=ARP_ANNOUNCE_NUM; i>0; i--)
	{
		// serialize MACRAW packet with ARP payload
		memcpy(&BUF_O_BASE(buf_o_ptr)[WIZ_SEND_OFFSET], &header, MACRAW_HEADER_SIZE);
		memcpy(&BUF_O_BASE(buf_o_ptr)[WIZ_SEND_OFFSET + MACRAW_HEADER_SIZE], &payload, ARP_PAYLOAD_SIZE);
		// send packet
		macraw_send(sock, BUF_O_BASE(buf_o_ptr), MACRAW_HEADER_SIZE + ARP_PAYLOAD_SIZE);

		// delay between announces
		if(i>1)
		{
			tick = systick_uptime();
			arp_timeout = SNTP_SYSTICK_RATE*ARP_ANNOUNCE_INTERVAL;
			while(systick_uptime() - tick < arp_timeout)
				; // wait for random delay
		}
	}
	
	// close MACRAW socket
	macraw_end(sock);
}

D arp/arp_private.h => arp/arp_private.h +0 -60
@@ 1,60 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _ARP_PRIVATE_H_
#define _ARP_PRIVATE_H_

#include <arp.h>

#define MACRAW_TYPE_ARP 0x0806

#define ARP_HTYPE_ETHERNET 0x0001
#define ARP_PTYPE_IPV4 0x0800
#define ARP_HLEN_ETHERNET 0x06
#define ARP_PLEN_IPV4 0x04

#define ARP_OPER_REQUEST 0x0001
#define ARP_OPER_REPLY 0x0002

#define ARP_PROBE_WAIT           1 // second  (initial random delay)
#define ARP_PROBE_NUM            3 //         (number of probe packets)
#define ARP_PROBE_MIN            1 // second  (minimum delay until repeated probe)
#define ARP_PROBE_MAX            2 // seconds (maximum delay until repeated probe)
#define ARP_ANNOUNCE_WAIT        2 // seconds (delay before announcing)
#define ARP_ANNOUNCE_NUM         2 //         (number of announcement packets)
#define ARP_ANNOUNCE_INTERVAL    2 // seconds (time between announcement packets)
#define ARP_MAX_CONFLICTS       10 //         (max conflicts before rate limiting)
#define ARP_RATE_LIMIT_INTERVAL 60 // seconds (delay between successive attempts)
#define ARP_DEFEND_INTERVAL     10 // seconds (minimum interval between defensive

typedef struct _ARP_Payload ARP_Payload;

struct _ARP_Payload {
	uint16_t htype;		// hardware type(ethernet MAC)
	uint16_t ptype;		// protocol type(IPv4|IPv6)
	uint8_t hlen;			// hardware length(6bytes)
	uint8_t plen;			// protocol length(4bytes for IPv4)
	uint16_t oper;		// operation mode(request|reply)
	uint8_t sha [6];	// source hardware address
	uint8_t spa [4];	// source IP address
	uint8_t tha [6];	// target hardware address
	uint8_t tpa [4];	// target IP address
} __attribute((packed,aligned(2)));

#define ARP_PAYLOAD_SIZE sizeof(ARP_Payload)

#endif // _ARP_PRIVATE_H

M chimutil/chimutil.c => chimutil/chimutil.c +2 -85
@@ 26,11 26,8 @@
#include <chimutil.h>
#include <tube.h>
#include <armfix.h>
#include <dhcpc.h>
#include <config.h>
#include <wiz.h>
#include <sntp.h>
#include <ptp.h>
#include <debug.h>

uint_fast8_t


@@ 134,56 131,6 @@ config_enable(uint8_t b)
}

void 
ptp_enable(uint8_t b)
{
	Socket_Config *event = &config.ptp.event;
	Socket_Config *general = &config.ptp.general;
		
	timer_pause(ptp_timer);

	event->enabled = b;
	general->enabled = b;
	udp_end(event->sock);
	udp_end(general->sock);

	if(event->enabled)
	{
		ptp_reset();

		udp_set_remote(event->sock, event->ip, event->port[DST_PORT]);
		udp_set_remote(general->sock, general->ip, general->port[DST_PORT]);

		udp_begin(event->sock, event->port[SRC_PORT],
			wiz_is_multicast(event->ip));
		udp_begin(general->sock, general->port[SRC_PORT],
			wiz_is_multicast(general->ip));
	}
}

void 
sntp_enable(uint8_t b)
{
	Socket_Config *socket = &config.sntp.socket;

	timer_pause(sync_timer);
	sync_timer_reconfigure();

	socket->enabled = b;
	udp_end(socket->sock);

	if(socket->enabled)
	{
		sntp_reset();

		udp_set_remote(socket->sock, socket->ip, socket->port[DST_PORT]);
		udp_begin(socket->sock, socket->port[SRC_PORT],
			wiz_is_multicast(socket->ip));

		timer_resume(sync_timer);
	}
}

void 
debug_enable(uint8_t b)
{
	Socket_Config *socket = &config.debug.osc.socket;


@@ 210,38 157,6 @@ debug_enable(uint8_t b)
	}
}

void 
mdns_enable(uint8_t b)
{
	Socket_Config *socket = &config.mdns.socket;

	socket->enabled = b;
	udp_end(socket->sock);

	if(socket->enabled)
	{
		udp_set_remote(socket->sock, socket->ip, socket->port[DST_PORT]);
		udp_begin(socket->sock, socket->port[SRC_PORT],
			wiz_is_multicast(socket->ip));
	}
}

void 
dhcpc_enable(uint8_t b)
{
	Socket_Config *socket = &config.dhcpc.socket;

	socket->enabled = b;
	udp_end(socket->sock);

	if(socket->enabled)
	{
		udp_set_remote(socket->sock, socket->ip, socket->port[DST_PORT]);
		udp_begin(socket->sock, socket->port[SRC_PORT],
			wiz_is_multicast(socket->ip));
	}
}

void
stop_watch_start(Stop_Watch *sw)
{


@@ 256,7 171,9 @@ stop_watch_stop(Stop_Watch *sw)

	if(sw->counter > sw->thresh)
	{
		/* FIXME
		DEBUG("ssi", "stop_watch", sw->id, sw->ticks * SNTP_SYSTICK_US / sw->thresh); // 1 tick = 100 us
		*/

		sw->ticks = 0;
		sw->counter = 0;

M cmc/cmc.c => cmc/cmc.c +0 -157
@@ 30,11 30,7 @@

// engines
#include <tuio2.h>
#include <tuio1.h>
#include <scsynth.h>
#include <oscmidi.h>
#include <dummy.h>
#include <custom.h>

// globals
CMC_Engine *engines [ENGINE_MAX+1];


@@ 300,135 296,6 @@ cmc_process(OSC_Timetag now, OSC_Timetag offset, int16_t *rela, osc_data_t *buf,
				}
				break;
			}
			case INTERPOLATION_CATMULL: // cubic interpolation: Catmull-Rom splines
			{
				float y0, y1, y2, y3, x1;

				float tm1 = vy[P-1];
				float thi = vy[P];
				float tp1 = vy[P+1];

				if(tm1 >= tp1)
				{
					x1 = vx[P-1];
					//y0 = vy[P-2];
					y0 = P >= 2 ? vy[P-2] : vy[P-1]; // check for underflow
					y1 = tm1;
					y2 = thi;
					y3 = tp1;
				}
				else // tp1 > tm1
				{
					x1 = vx[P];
					y0 = tm1;
					y1 = thi;
					y2 = tp1;
					//y3 = vy[P+2];
					y3 = P <= (SENSOR_N) ? vy[P+2] : vy[P+1]; // check for overflow
				}

				y0 = y0 < 0.f ? 0.f :(y0 > 1.f ? 1.f : y0);
				y1 = y1 < 0.f ? 0.f :(y1 > 1.f ? 1.f : y1);
				y2 = y2 < 0.f ? 0.f :(y2 > 1.f ? 1.f : y2);
				y3 = y3 < 0.f ? 0.f :(y3 > 1.f ? 1.f : y3);

				// lookup distance
				y0 = LOOKUP(y0);
				y1 = LOOKUP(y1);
				y2 = LOOKUP(y2);
				y3 = LOOKUP(y3);

				// simple cubic splines
				//float a0 = y3 - y2 - y0 + y1;
				//float a1 = y0 - y1 - a0;
				//float a2 = y2 - y0;
				//float a3 = y1;
			
				// catmull-rom splines
				float a0 = -0.5f*y0 + 1.5f*y1 - 1.5f*y2 + 0.5f*y3;
				float a1 = y0 - 2.5f*y1 + 2.f*y2 - 0.5f*y3;
				float a2 = -0.5f*y0 + 0.5f*y2;
				float a3 = y1;

        float A = 3.f * a0;
        float B = 2.f * a1;
        float C = a2;

				float mu;

        if(A == 0.f)
        {
          mu = 0.f; // TODO what to do here? fall back to quadratic?
        }
        else // A != 0.f
        {
          if(C == 0.f)
            mu = -B / A;
          else
          {
            float A2 = 2.f*A;
            float D = B*B - 2.f*A2*C;
            if(D < 0.f) // bad, this'd give an imaginary solution
              D = 0.f;
            else
              D = sqrtf(D);
            mu =(-B - D) / A2;
          }
        }

				x = x1 + mu*d;
				float mu2 = mu*mu;
				y = a0*mu2*mu + a1*mu2 + a2*mu + a3;

				break;
			}
			case INTERPOLATION_LAGRANGE: // cubic interpolation: Lagrange Poylnomial
			{
				float y0, y1, y2, y3, x1;

				x1 = vx[P];
				y0 = vy[P-1];
				y1 = vy[P];
				y2 = vy[P+1];
				//y3 = vy[P+2];
				y3 = P <= (SENSOR_N) ? vy[P+2] : vy[P+1]; // check for overflow

				y0 = y0 < 0.f ? 0.f :(y0 > 1.f ? 1.f : y0);
				y1 = y1 < 0.f ? 0.f :(y1 > 1.f ? 1.f : y1);
				y2 = y2 < 0.f ? 0.f :(y2 > 1.f ? 1.f : y2);
				y3 = y3 < 0.f ? 0.f :(y3 > 1.f ? 1.f : y3);

				// lookup distance
				y0 = LOOKUP(y0);
				y1 = LOOKUP(y1);
				y2 = LOOKUP(y2);
				y3 = LOOKUP(y3);

				float d2 = d * d;
				float d3 = d2 * d;

				float s1 = y0 - 2.f*y1 + y2;
				float s2 = y0 - 3.f*y1 + 3.f*y2 - y3;
				float sq = y0*(y0 - 9.f*y1 + 6.f*y2 + y3) + y1*(21.f*y1 - 39.f*y2 + 6.f*y3) + y2*(21.f*y2 - 9.f*y3) + y3*y3;

				if(sq < 0.f) // bad, this'd give an imaginary solution
					sq = 0.f;

				if(s2 == 0)
					x = x1; //FIXME what to do here?
				else
					x =(3.f*d*s1 + 3.f*x1*s2 + sqrtf(3.f)*d*sqrtf(
						y0*(y0 - 9.f*y1 + 6.f*y2 + y3) + y1*(21.f*y1 - 39.f*y2 + 6.f*y3) + y2*(21.f*y2 - 9.f*y3) + y3*y3
					)) /(3.f*s2);

				float X1 = x - x1;
				float X2 = X1 * X1;
				float X3 = X2 * X1;

				y = -(-6.f*d3*y1 + d2*X1*(2.f*y0 + 3.f*y1 - 6.f*y2 + y3) - 3.f*d*X2*s1 + X3*s2) /(6.f*d3);

				break;
			}
			default:
				x = 0.f;
				y = 0.f;


@@ 803,20 670,8 @@ cmc_engines_init(void)
	if(oscmidi_engine.init_cb)
		oscmidi_engine.init_cb();

	if(dummy_engine.init_cb)
		dummy_engine.init_cb();

	if(scsynth_engine.init_cb)
		scsynth_engine.init_cb();

	if(tuio2_engine.init_cb)
		tuio2_engine.init_cb();

	if(tuio1_engine.init_cb)
		tuio1_engine.init_cb();

	if(custom_engine.init_cb)
		custom_engine.init_cb();
}

void


@@ 843,20 698,8 @@ cmc_engines_update(void)
	if(config.oscmidi.enabled)
		engines[cmc_engines_active++] = &oscmidi_engine;

	if(config.dummy.enabled)
		engines[cmc_engines_active++] = &dummy_engine;

	if(config.scsynth.enabled)
		engines[cmc_engines_active++] = &scsynth_engine;

	if(config.tuio2.enabled)
		engines[cmc_engines_active++] = &tuio2_engine;

	if(config.tuio1.enabled)
		engines[cmc_engines_active++] = &tuio1_engine;

	if(config.custom.enabled)
		engines[cmc_engines_active++] = &custom_engine;

	engines[cmc_engines_active] = NULL;
}

M config/config.c => config/config.c +3 -190
@@ 29,12 29,7 @@
#include <cmc.h>
#include <midi.h>
#include <calibration.h>
#include <sntp.h>
#include <ptp.h>
#include <engines.h>
#include <ipv4ll.h>
#include <dhcpc.h>
#include <mdns-sd.h>
#include <debug.h>
#include <sensors.h>



@@ 47,13 42,13 @@ static const char *local_str = ".local";

static const Socket_Enable_Cb socket_callbacks [WIZ_MAX_SOCK_NUM] = {
	[SOCK_DHCPC]	= NULL, // = SOCK_ARP
	[SOCK_SNTP]		= sntp_enable,
	[SOCK_PTP_EV]	= ptp_enable,
	[SOCK_SNTP]		= NULL,
	[SOCK_PTP_EV]	= NULL,
	[SOCK_PTP_GE]	= NULL,
	[SOCK_OUTPUT]	= output_enable,
	[SOCK_CONFIG]	= config_enable,
	[SOCK_DEBUG]	= debug_enable,
	[SOCK_MDNS]		= mdns_enable,
	[SOCK_MDNS]		= NULL
};

Config config = {


@@ 79,20 74,10 @@ Config config = {
		.derivatives = 0
	},

	.tuio1 = {
		.enabled = 0,
		.custom_profile = 0
	},

	.dump = {
		.enabled = 0
	},

	.scsynth = {
		.enabled = 0,
		.derivatives = 0
	},

	.oscmidi = {
		.enabled = 0,
		.multi = 1,


@@ 100,26 85,6 @@ Config config = {
		.mpe = 0,
		.path = {'/', 'm', 'i', 'd', 'i', '\0'}
	},

	.dummy = {
		.enabled = 0,
		.redundancy = 0,
		.derivatives = 0
	},

	.custom = {
		.enabled = 0,
		/*
		.items = {
			[0] = {
				.dest = RPN_NONE,
				.path = {'\0'},
				.fmt = {'\0'},
				.vm = { .inst = { RPN_TERMINATOR } }
			}
		}
		*/
	},
	
	.output = {
		.osc = {


@@ 132,7 97,6 @@ Config config = {
			.mode = OSC_MODE_UDP,
			.server = 0
		},
		.offset = 0.002ULLK, // := 2ms offset
		.invert = {
			.x = 0,
			.z = 0


@@ 153,34 117,6 @@ Config config = {
		}
	},

	.ptp = {
		.multiplier = 4, // # of sync messages between PTP delay requests 
		.offset_stiffness = 16,
		.delay_stiffness = 16,
		.event = {
			.sock = SOCK_PTP_EV,
			.enabled = 0,
			.port = {319, 319},
			.ip = {224, 0, 1, 129} // PTPv2 multicast group, 224.0.0.107 for peer-delay measurements
		},
		.general = {
			.sock = SOCK_PTP_GE,
			.enabled = 0,
			.port = {320, 320},
			.ip = {224, 0, 1, 129} // PTPv2 multicast group, 224.0.0.107 for peer-delay measurements
		}
	},

	.sntp = {
		.tau = 4, // delay between SNTP requests in seconds
		.socket = {
			.sock = SOCK_SNTP,
			.enabled = 0,
			.port = {123, 123},
			.ip = IP_BROADCAST
		}
	},

	.debug = {
		.osc = {
			.socket = {


@@ 194,28 130,6 @@ Config config = {
		}
	},

	.ipv4ll = {
		.enabled = 1
	},

	.mdns = {
		.socket = {
			.sock = SOCK_MDNS,
			.enabled = 1,
			.port = {5353, 5353}, // mDNS multicast port
			.ip = {224, 0, 0, 251} // mDNS multicast group
		}
	},

	.dhcpc = {
		.socket = {
			.sock = SOCK_DHCPC,
			.enabled = 0,
			.port = {68, 67}, // BOOTPclient, BOOTPserver
			.ip = IP_BROADCAST
		}
	},

	.sensors = {
		.movingaverage_bitshift = 3,
		.interpolation_mode = INTERPOLATION_QUADRATIC,


@@ 241,30 155,6 @@ Config config = {
		}
	},

#define SYNTH_DEF(C, N) \
		{ \
			.name = {'s', 'y', 'n', 't', 'h', '_', C, '\0'}, \
			.sid = 200, \
			.group = 100 + N, \
			.out = N, \
			.arg = 0, \
			.alloc = 1, \
			.gate = 1, \
			.add_action = SCSYNTH_ADD_TO_HEAD, \
			.is_group = 0 \
		}
	.scsynth_groups = {
		[0] = SYNTH_DEF('0', 0),
		[1] = SYNTH_DEF('1', 1),
		[2] = SYNTH_DEF('2', 2),
		[3] = SYNTH_DEF('3', 3),
		[4] = SYNTH_DEF('4', 4),
		[5] = SYNTH_DEF('5', 5),
		[6] = SYNTH_DEF('6', 6),
		[7] = SYNTH_DEF('7', 7)
	},
#undef SYNTH_DEF

#define MIDI_DEF \
		{ \
			.mapping = OSC_MIDI_MAPPING_CONTROL_CHANGE, \


@@ 532,10 422,6 @@ _info_name(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf
		buf_ptr = osc_get_string(buf_ptr, &s);
		strcpy(config.name, s);

		//FIXME we should do a probe to look for an existing name
		if(config.mdns.socket.enabled)
			mdns_update(); // announce new name

		size = CONFIG_SUCCESS("is", uuid, path);
	}



@@ 673,11 559,6 @@ _comm_ip(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
				wiz_gateway_set(config.comm.gateway);
			}

			//FIXME disrupts PTP
			//FIXME we should do a probe to look for an existing name
			if(config.mdns.socket.enabled)
				mdns_update(); // announce new IP

			size = CONFIG_SUCCESS("is", uuid, path);
		}
		else


@@ 767,11 648,7 @@ _output_reset(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *

	config.dump.enabled = 0;
	config.tuio2.enabled = 0;
	config.tuio1.enabled = 0;
	config.scsynth.enabled = 0;
	config.oscmidi.enabled = 0;
	config.dummy.enabled = 0;
	config.custom.enabled = 0;

	cmc_engines_update();



@@ 914,9 791,6 @@ config_address(Socket_Config *socket, const char *path, const char *fmt, uint_fa
				*port_str = 0x0; // end name here
				port = atoi(port_str+1);
				address_cb.port = port;

				if(!mdns_resolve(hostname, _address_dns_cb, &address_cb))
					size = CONFIG_FAIL("iss", uuid, path, "there is a mDNS request already ongoing");
			}
			else
				size = CONFIG_FAIL("iss", uuid, path, "can only resolve raw IP and mDNS addresses");


@@ 948,9 822,6 @@ _config_mode(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *b
	}
	else
	{
		if(config.mdns.socket.enabled)
			mdns_goodbye(); // invalidate currently registered services

		// XXX need to send reply before disabling socket...
		size = CONFIG_SUCCESS("is", uuid, path);
		CONFIG_SEND(size);


@@ 966,9 837,6 @@ _config_mode(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *b
				break;
			}
		config_enable(enabled);

		if(config.mdns.socket.enabled)
			mdns_announce(); // announce new mode
	}

	return 1;


@@ 981,34 849,6 @@ _output_address(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t
}

static uint_fast8_t
_output_offset(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	(void)fmt;
	osc_data_t *buf_ptr = buf;
	uint16_t size;
	int32_t uuid;

	buf_ptr = osc_get_int32(buf_ptr, &uuid);

	if(argc == 1) // query
	{
		float f = config.output.offset;
		size = CONFIG_SUCCESS("isf", uuid, path, f); // output timestamp, double, float?
	}
	else
	{
		float f;
		buf_ptr = osc_get_float(buf_ptr, &f);
		config.output.offset = f;
		size = CONFIG_SUCCESS("is", uuid, path);
	}

	CONFIG_SEND(size);

	return 1;
}

static uint_fast8_t
_output_invert_x(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	(void)fmt;


@@ 1081,9 921,6 @@ _reset_soft(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *bu

	buf_ptr = osc_get_int32(buf_ptr, &uuid);

	if(config.mdns.socket.enabled)
		mdns_goodbye(); // invalidate currently registered IP and services

	size = CONFIG_SUCCESS("is", uuid, path);
	CONFIG_SEND(size);



@@ 1108,9 945,6 @@ _reset_hard(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *bu

	buf_ptr = osc_get_int32(buf_ptr, &uuid);

	if(config.mdns.socket.enabled)
		mdns_goodbye(); // invalidate currently registered IP and services

	size = CONFIG_SUCCESS("is", uuid, path);
	CONFIG_SEND(size);



@@ 1135,9 969,6 @@ _reset_flash(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *b

	buf_ptr = osc_get_int32(buf_ptr, &uuid);

	if(config.mdns.socket.enabled)
		mdns_goodbye(); // invalidate currently registered IP and services

	size = CONFIG_SUCCESS("is", uuid, path);
	CONFIG_SEND(size);



@@ 1191,10 1022,6 @@ static const OSC_Query_Argument comm_gateway_args [] = {
	OSC_QUERY_ARGUMENT_STRING("32-bit decimal dotted or mDNS .local domain", OSC_QUERY_MODE_RW, 64)
};

static const OSC_Query_Argument comm_address_args [] = {
	OSC_QUERY_ARGUMENT_STRING("32-bit decimal dotted or mDNS .local domain", OSC_QUERY_MODE_W, 64)
};

const OSC_Query_Item comm_tree [] = {
	OSC_QUERY_ITEM_METHOD("mac", "Hardware MAC address", _comm_mac, comm_mac_args),
	OSC_QUERY_ITEM_METHOD("ip", "IPv4 client address", _comm_ip, comm_ip_args),


@@ 1233,10 1060,6 @@ static const OSC_Query_Item info_tree [] = { //FIXME merge with comm?
	OSC_QUERY_ITEM_METHOD("name", "Device name", _info_name, info_name_args),
};

static const OSC_Query_Argument engines_offset_args [] = {
	OSC_QUERY_ARGUMENT_FLOAT("Seconds", OSC_QUERY_MODE_RW, 0.f, 10.f, 0.0001f)
};

static const OSC_Query_Argument engines_invert_args [] = {
	OSC_QUERY_ARGUMENT_BOOL("axis inversion", OSC_QUERY_MODE_RW)
};


@@ 1244,7 1067,6 @@ static const OSC_Query_Argument engines_invert_args [] = {
static const OSC_Query_Item engines_tree [] = {
	OSC_QUERY_ITEM_METHOD("enabled", "Enable/disable", _output_enabled, config_boolean_args),
	OSC_QUERY_ITEM_METHOD("address", "Single remote host", _output_address, config_address_args),
	OSC_QUERY_ITEM_METHOD("offset", "OSC bundle offset timestamp", _output_offset, engines_offset_args),
	OSC_QUERY_ITEM_METHOD("invert_x", "Enable/disable x-axis inversion", _output_invert_x, engines_invert_args),
	OSC_QUERY_ITEM_METHOD("invert_z", "Enable/disable z-axis inversion", _output_invert_z, engines_invert_args),
	OSC_QUERY_ITEM_METHOD("parallel", "Parallel processing", _output_parallel, config_boolean_args),


@@ 1254,12 1076,8 @@ static const OSC_Query_Item engines_tree [] = {

	// engines
	OSC_QUERY_ITEM_NODE("dump/", "Dump output engine", dump_tree),
	OSC_QUERY_ITEM_NODE("dummy/", "Dummy output engine", dummy_tree),
	OSC_QUERY_ITEM_NODE("oscmidi/", "OSC MIDI output engine", oscmidi_tree),
	OSC_QUERY_ITEM_NODE("scsynth/", "SuperCollider output engine", scsynth_tree),
	OSC_QUERY_ITEM_NODE("tuio2/", "TUIO 2.0 output engine", tuio2_tree),
	OSC_QUERY_ITEM_NODE("tuio1/", "TUIO 1.0 output engine", tuio1_tree),
	OSC_QUERY_ITEM_NODE("custom/", "Custom output engine", custom_tree)
};

static const OSC_Query_Item root_tree [] = {


@@ 1270,12 1088,7 @@ static const OSC_Query_Item root_tree [] = {

	// sockets
	OSC_QUERY_ITEM_NODE("config/", "Configuration", config_tree),
	OSC_QUERY_ITEM_NODE("sntp/", "Simplified Network Time Protocol", sntp_tree),
	OSC_QUERY_ITEM_NODE("ptp/", "Precision Time Protocol", ptp_tree),
	OSC_QUERY_ITEM_NODE("ipv4ll/", "IPv4 Link Local Addressing", ipv4ll_tree),
	OSC_QUERY_ITEM_NODE("dhcpc/", "DHCP Client", dhcpc_tree),
	OSC_QUERY_ITEM_NODE("debug/", "Debug", debug_tree),
	OSC_QUERY_ITEM_NODE("mdns/", "Multicast DNS", mdns_tree),

	// output engines
	OSC_QUERY_ITEM_NODE("engines/", "Output engines", engines_tree),

D custom/custom.c => custom/custom.c +0 -394
@@ 1,394 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#include <string.h>
#include <math.h> // floor

#include <chimaera.h>
#include <chimutil.h>
#include <config.h>
#include <cmc.h>

#include "custom_private.h"

static Custom_Item *items = config.custom.items;
static RPN_Stack stack;

static osc_data_t *pack;
static osc_data_t *bndl;

static osc_data_t *
custom_engine_frame_cb(osc_data_t *buf, osc_data_t *end, CMC_Frame_Event *fev)
{
	stack.fid = fev->fid;
	stack.sid = stack.gid = stack.pid = 0;
	stack.x = stack.z = 0.f;
	stack.vx = stack.vz = 0.f;

	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;

	if(cmc_engines_active + config.dump.enabled > 1)
		buf_ptr = osc_start_bundle_item(buf_ptr, end, &pack);
	buf_ptr = osc_start_bundle(buf_ptr, end, fev->offset, &bndl);

	uint_fast8_t i;
	Custom_Item *item;
	if(fev->nblob_old + fev->nblob_new)
	{
		for(i=0; i<CUSTOM_MAX_EXPR; i++)
		{
			item = &items[i];
			if(item->dest == RPN_FRAME)
			{
				buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
				{
					buf_ptr = osc_set_path(buf_ptr, end, item->path);
					buf_ptr = osc_set_fmt(buf_ptr, end, item->fmt);

					buf_ptr = rpn_run(buf_ptr, end, item, &stack);
				}
				buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);
			}
			else if(item->dest == RPN_NONE)
				break;
		}
	}
	else
	{
		for(i=0; i<CUSTOM_MAX_EXPR; i++)
		{
			item = &items[i];
			if(item->dest == RPN_IDLE)
			{
				buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
				{
					buf_ptr = osc_set_path(buf_ptr, end, item->path);
					buf_ptr = osc_set_fmt(buf_ptr, end, item->fmt);

					buf_ptr = rpn_run(buf_ptr, end, item, &stack);
				}
				buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);
			}
			else if(item->dest == RPN_NONE)
				break;
		}
	}

	return buf_ptr;
}

static osc_data_t *
custom_engine_end_cb(osc_data_t *buf, osc_data_t *end, CMC_Frame_Event *fev)
{
	(void)fev;
	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;

	uint_fast8_t i;
	Custom_Item *item;
	for(i=0; i<CUSTOM_MAX_EXPR; i++)
	{
		item = &items[i];
		if(item->dest == RPN_END)
		{
			buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
			{
				buf_ptr = osc_set_path(buf_ptr, end, item->path);
				buf_ptr = osc_set_fmt(buf_ptr, end, item->fmt);

				buf_ptr = rpn_run(buf_ptr, end, item, &stack);
			}
			buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);
		}
		else if(item->dest == RPN_NONE)
			break;
	}

	buf_ptr = osc_end_bundle(buf_ptr, end, bndl);
	if(cmc_engines_active + config.dump.enabled > 1)
		buf_ptr = osc_end_bundle_item(buf_ptr, end, pack);

	return buf_ptr;
}

static osc_data_t *
custom_engine_on_cb(osc_data_t *buf, osc_data_t *end, CMC_Blob_Event *bev)
{
	stack.sid = bev->sid;
	stack.gid = bev->gid;
	stack.pid = bev->pid;
	stack.x = bev->x;
	stack.z = bev->y;
	stack.vx = bev->vx;
	stack.vz = bev->vy;

	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;

	uint_fast8_t i;
	Custom_Item *item;
	for(i=0; i<CUSTOM_MAX_EXPR; i++)
	{
		item = &items[i];
		if(item->dest == RPN_ON)
		{
			buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
			{
				buf_ptr = osc_set_path(buf_ptr, end, item->path);
				buf_ptr = osc_set_fmt(buf_ptr, end, item->fmt);

				buf_ptr = rpn_run(buf_ptr, end, item, &stack);
			}
			buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);
		}
		else if(item->dest == RPN_NONE)
			break;
	}
	
	return buf_ptr;
}

static osc_data_t *
custom_engine_off_cb(osc_data_t *buf, osc_data_t *end, CMC_Blob_Event *bev)
{
	stack.sid = bev->sid;
	stack.gid = bev->gid;
	stack.pid = bev->pid;
	stack.x = stack.z = 0.f;
	stack.vx = stack.vz = 0.f;

	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;

	uint_fast8_t i;
	Custom_Item *item;
	for(i=0; i<CUSTOM_MAX_EXPR; i++)
	{
		item = &items[i];
		if(item->dest == RPN_OFF)
		{
			buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
			{
				buf_ptr = osc_set_path(buf_ptr, end, item->path);
				buf_ptr = osc_set_fmt(buf_ptr, end, item->fmt);

				buf_ptr = rpn_run(buf_ptr, end, item, &stack);
			}
			buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);
		}
		else if(item->dest == RPN_NONE)
			break;
	}
	
	return buf_ptr;
}

static osc_data_t *
custom_engine_set_cb(osc_data_t *buf, osc_data_t *end, CMC_Blob_Event *bev)
{
	stack.sid = bev->sid;
	stack.gid = bev->gid;
	stack.pid = bev->pid;
	stack.x = bev->x;
	stack.z = bev->y;
	stack.vx = bev->vx;
	stack.vz = bev->vy;

	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;

	uint_fast8_t i;
	Custom_Item *item;
	for(i=0; i<CUSTOM_MAX_EXPR; i++)
	{
		item = &items[i];
		if(item->dest == RPN_SET)
		{
			buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
			{
				buf_ptr = osc_set_path(buf_ptr, end, item->path);
				buf_ptr = osc_set_fmt(buf_ptr, end, item->fmt);

				buf_ptr = rpn_run(buf_ptr, end, item, &stack);
			}
			buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);
		}
		else if(item->dest == RPN_NONE)
			break;
	}
	
	return buf_ptr;
}

CMC_Engine custom_engine = {
	NULL,
	custom_engine_frame_cb,
	custom_engine_on_cb,
	custom_engine_off_cb,
	custom_engine_set_cb,
	custom_engine_end_cb
};

/*
 * Config
 */
static uint_fast8_t
_custom_enabled(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	uint_fast8_t res = config_check_bool(path, fmt, argc, buf, &config.custom.enabled);
	cmc_engines_update();
	return res;
}

static uint_fast8_t
_custom_reset(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	(void)fmt;
	(void)argc;
	osc_data_t *buf_ptr = buf;
	uint16_t size;
	int32_t uuid;

	buf_ptr = osc_get_int32(buf_ptr, &uuid);

	uint_fast8_t i;
	for(i=0; i<CUSTOM_MAX_EXPR; i++)
	{
		Custom_Item *item = &items[i];

		item->dest = RPN_NONE;
		item->path[0] = '\0';
		item->fmt[0] = '\0';
		item->vm.inst[0] = RPN_TERMINATOR;
	}

	size = CONFIG_SUCCESS("is", uuid, path);
	CONFIG_SEND(size);
	return 1;
}

static const OSC_Query_Value custom_append_destination_args_values [] = {
	[RPN_FRAME]	= { .s = "frame" },
	[RPN_ON]	= { .s = "on" },
	[RPN_OFF]	= { .s = "off" },
	[RPN_SET]	= { .s = "set" },
	[RPN_END]	= { .s = "end" },
	[RPN_IDLE]	= { .s = "idle" }
};

static uint_fast8_t
_custom_append(int dest, const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	(void)fmt;
	osc_data_t *buf_ptr = buf;
	uint16_t size = 0;
	int32_t uuid;

	buf_ptr = osc_get_int32(buf_ptr, &uuid);

	if(argc == 1)
	{
		//TODO this is write only for now 
		//size = CONFIG_SUCCESS("isss", uuid, path, item->path, item->fmt);
	}
	else
	{
		uint_fast8_t i;
		Custom_Item *item;
		for(i=0; i<CUSTOM_MAX_EXPR; i++)
		{
			item = &items[i];
			if(item->dest == RPN_NONE)
				break;
		}

		const char *argv;
		buf_ptr = osc_get_string(buf_ptr, &argv);

		if( (item->dest == RPN_NONE) && rpn_compile(argv, item) )
		{
			item->dest = dest;
			size = CONFIG_SUCCESS("is", uuid, path);
		}
		else
			size = CONFIG_FAIL("iss", uuid, path, "parse error and/or stack under/overflow");
	}

	CONFIG_SEND(size);

	return 1;
}

static uint_fast8_t
_custom_append_frame(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return _custom_append(RPN_FRAME, path, fmt, argc, buf);
}

static uint_fast8_t
_custom_append_on(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return _custom_append(RPN_ON, path, fmt, argc, buf);
}

static uint_fast8_t
_custom_append_off(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return _custom_append(RPN_OFF, path, fmt, argc, buf);
}

static uint_fast8_t
_custom_append_set(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return _custom_append(RPN_SET, path, fmt, argc, buf);
}

static uint_fast8_t
_custom_append_end(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return _custom_append(RPN_END, path, fmt, argc, buf);
}

static uint_fast8_t
_custom_append_idle(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return _custom_append(RPN_IDLE, path, fmt, argc, buf);
}

static const OSC_Query_Argument custom_append_args [] = {
	OSC_QUERY_ARGUMENT_STRING("Postfix code", OSC_QUERY_MODE_W, CUSTOM_ARGS_LEN)
};

const OSC_Query_Item custom_append_tree [] = {
	OSC_QUERY_ITEM_METHOD("frame", "Append frame hook", _custom_append_frame, custom_append_args),
	OSC_QUERY_ITEM_METHOD("on", "Append on hook", _custom_append_on, custom_append_args),
	OSC_QUERY_ITEM_METHOD("off", "Append off hook", _custom_append_off, custom_append_args),
	OSC_QUERY_ITEM_METHOD("set", "Append set hook", _custom_append_set, custom_append_args),
	OSC_QUERY_ITEM_METHOD("end", "Append end hook", _custom_append_end, custom_append_args),
	OSC_QUERY_ITEM_METHOD("idle", "Append idle hook", _custom_append_idle, custom_append_args),
};

/*
 * Query
 */

const OSC_Query_Item custom_tree [] = {
	OSC_QUERY_ITEM_METHOD("enabled", "Enable/disable", _custom_enabled, config_boolean_args),
	OSC_QUERY_ITEM_METHOD("reset", "Reset", _custom_reset, NULL),
	OSC_QUERY_ITEM_NODE("append/", "Append hook", custom_append_tree),
};

D custom/custom_private.h => custom/custom_private.h +0 -56
@@ 1,56 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _CUSTOM_PRIVATE_H_
#define _CUSTOM_PRIVATE_H_

#include <custom.h>

#define RPN_STACK_HEIGHT 16
#define RPN_REG_HEIGHT 8

typedef struct _RPN_Stack RPN_Stack;
typedef struct _RPN_Compiler RPN_Compiler;

struct _RPN_Stack {
	uint32_t fid;
	//OSC_Timetag now;
	//OSC_Timetag offset;
	//uint_fast8_t old_n;
	//uint_fast8_t new_n;
	uint32_t sid;
	uint16_t gid;
	uint16_t pid;
	float x;
	float z;
	float vx;
	float vz;

	float reg [RPN_REG_HEIGHT]; //FIXME use MAX_BLOB instead?
	float arr [RPN_STACK_HEIGHT];
	float *ptr;
};

struct _RPN_Compiler {
	uint_fast8_t offset;
	int_fast8_t pp;
};

osc_data_t *rpn_run(osc_data_t *buf, osc_data_t *end, Custom_Item *itm, RPN_Stack *stack);
uint_fast8_t rpn_compile(const char *args, Custom_Item *itm);

#endif // _CUSTOM_PRIVATE_H_

D custom/custom_rpn.c => custom/custom_rpn.c +0 -759
@@ 1,759 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#include <string.h>

#include "custom_private.h"

static inline __always_inline float
pop(RPN_Stack *stack)
{
	(stack->ptr)--; // underflow is checked for at compile time
	float v = *(stack->ptr);
	return v;
}

static inline __always_inline void
push(RPN_Stack *stack, float v)
{
	*(stack->ptr) = v;
	(stack->ptr)++; // overflow is checked for at compile time
}

static inline __always_inline void
xchange(RPN_Stack *stack)
{
	float b = pop(stack);
	float a = pop(stack);
	push(stack, b);
	push(stack, a);
}

static inline __always_inline void
duplicate(RPN_Stack *stack, int32_t pos)
{
	float v = *(stack->ptr - pos);
	push(stack, v);
}

osc_data_t *
rpn_run(osc_data_t *buf, osc_data_t *end, Custom_Item *itm, RPN_Stack *stack)
{
	osc_data_t *buf_ptr = buf;
	RPN_VM *vm = &itm->vm;

	stack->ptr = stack->arr; // reset stack

	RPN_Instruction *inst;
	for(inst = vm->inst; *inst != RPN_TERMINATOR; inst++)
		switch(*inst)
		{
			case RPN_PUSH_VALUE:
			{
				push(stack, vm->val[inst - vm->inst]);
				break;
			}
			case RPN_POP_INT32:
			{
				volatile int32_t i = pop(stack);
				buf_ptr = osc_set_int32(buf_ptr, end, i);
				break;
			}
			case RPN_POP_FLOAT:
			{
				float f = pop(stack);
				buf_ptr = osc_set_float(buf_ptr, end, f);
				break;
			}
			case RPN_POP_MIDI:
			{
				uint8_t *m;
				buf_ptr = osc_set_midi_inline(buf_ptr, end, &m);
				if(buf_ptr)
				{
					m[3] = pop(stack);
					m[2] = pop(stack);
					m[1] = pop(stack);
					m[0] = pop(stack);
				}
				break;
			}

			case RPN_PUSH_FID:
			{
				push(stack, stack->fid);
				break;
			}
			case RPN_PUSH_SID:
			{
				push(stack, stack->sid);
				break;
			}
			case RPN_PUSH_GID:
			{
				push(stack, stack->gid);
				break;
			}
			case RPN_PUSH_PID:
			{
				push(stack, stack->pid);
				break;
			}
			case RPN_PUSH_X:
			{
				push(stack, stack->x);
				break;
			}
			case RPN_PUSH_Z:
			{
				push(stack, stack->z);
				break;
			}
			case RPN_PUSH_VX:
			{
				push(stack, stack->vx);
				break;
			}
			case RPN_PUSH_VZ:
			{
				push(stack, stack->vz);
				break;
			}
			case RPN_PUSH_N:
			{
				push(stack, SENSOR_N);
				break;
			}

			case RPN_PUSH_REG:
			{
				int32_t pos = pop(stack);
				float c = pop(stack);
				if(pos < RPN_REG_HEIGHT)
					stack->reg[pos] = c;
				else
				{
					; //TODO warn
				}
				break;
			}
			case RPN_POP_REG:
			{
				int32_t pos = pop(stack);
				if(pos < RPN_REG_HEIGHT)
					push(stack, stack->reg[pos]);
				else // TODO warn
					push(stack, NAN);
				break;
			}

			// standard operators
			case RPN_ADD:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a + b;
				push(stack, c);
				break;
			}
			case RPN_SUB:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a - b;
				push(stack, c);
				break;
			}
			case RPN_MUL:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a * b;
				push(stack, c);
				break;
			}
			case RPN_DIV:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a / b;
				push(stack, c);
				break;
			}
			case RPN_MOD:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = fmod(a, b);
				push(stack, c);
				break;
			}
			case RPN_POW:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = pow(a, b);
				push(stack, c);
				break;
			}
			case RPN_NEG:
			{
				float c = pop(stack);
				push(stack, -c);
				break;
			}
			case RPN_XCHANGE:
			{
				xchange(stack);
				break;
			}
			case RPN_DUPL_AT:
			{
				int32_t pos = pop(stack);
				if(pos > RPN_STACK_HEIGHT)
					pos = RPN_STACK_HEIGHT;
				else if(pos < 1)
					pos = 1;
				duplicate(stack, pos);
				break;
			}
			case RPN_DUPL_TOP:
			{
				duplicate(stack, 1);
				break;
			}
			case RPN_LSHIFT:
			{
				int32_t b = pop(stack);
				int32_t a = pop(stack);
				int32_t c = a << b;
				push(stack, c);
				break;
			}
			case RPN_RSHIFT:
			{
				int32_t b = pop(stack);
				int32_t a = pop(stack);
				int32_t c = a >> b;
				push(stack, c);
				break;
			}
			case RPN_LOGICAL_AND:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a && b;
				push(stack, c);
				break;
			}
			case RPN_BITWISE_AND:
			{
				int32_t b = pop(stack);
				int32_t a = pop(stack);
				int32_t c = a & b;
				push(stack, c);
				break;
			}
			case RPN_LOGICAL_OR:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a || b;
				push(stack, c);
				break;
			}
			case RPN_BITWISE_OR:
			{
				int32_t b = pop(stack);
				int32_t a = pop(stack);
				int32_t c = a | b;
				push(stack, c);
				break;
			}

			// conditionals
			case RPN_NOT:
			{
				float c = pop(stack);
				push(stack, !c);
				break;
			}
			case RPN_NOTEQ:
			{
				float a = pop(stack);
				float b = pop(stack);
				float c = a != b;
				push(stack, c);
				break;
			}
			case RPN_COND:
			{
				float c = pop(stack);
				if(!c)
					xchange(stack);
				pop(stack);
				break;
			}
			case RPN_LT:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a < b;
				push(stack, c);
				break;
			}
			case RPN_LEQ:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a <= b;
				push(stack, c);
				break;
			}
			case RPN_GT:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a > b;
				push(stack, c);
				break;
			}
			case RPN_GEQ:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a >= b;
				push(stack, c);
				break;
			}
			case RPN_EQ:
			{
				float b = pop(stack);
				float a = pop(stack);
				float c = a == b;
				push(stack, c);
				break;
			}
			case RPN_TERMINATOR:
				// never reached
				break;
		}
	
	return buf_ptr;
}

static uint_fast8_t
rpn_add_inst(RPN_VM *vm, RPN_Compiler *compiler, RPN_Instruction inst, float val, uint_fast8_t pops, uint_fast8_t pushs)
{
	//check for stack underflow
	compiler->pp -= pops;
	if(compiler->pp < 0)
		return 0;

	// check for stack overflow
	compiler->pp += pushs;
	if(compiler->pp > RPN_STACK_HEIGHT)
		return 0;

	vm->inst[compiler->offset] = inst;
	vm->val[compiler->offset] = val;

	if(compiler->offset >= CUSTOM_MAX_INST)
		return 0;
	compiler->offset++;

	return 1;
}

static uint_fast8_t
rpn_compile_sub(const char *str, size_t len, RPN_VM *vm, RPN_Compiler *compiler)
{
	const char *ptr = str;
	const char *end = str + len;

	while(ptr < end)
	{
		switch(*ptr)
		{
			case '$':
			{
				switch(ptr[1])
				{
					case 'f':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_FID, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					case 'b':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_SID, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					case 'g':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_GID, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					case 'p':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_PID, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					case 'x':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_X, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					case 'z':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_Z, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					case 'X':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_VX, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					case 'Z':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_VZ, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					case 'n':
						if(!rpn_add_inst(vm, compiler, RPN_PUSH_N, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					default:
						return 0; // parse error
				}
				ptr++;
				break;
			}

			case '[':
				if(!rpn_add_inst(vm, compiler, RPN_PUSH_REG, 0.f, 2, 0)) return 0;
				ptr++;
				break;
			case ']':
				if(!rpn_add_inst(vm, compiler, RPN_POP_REG, 0.f, 1, 1)) return 0;
				ptr++;
				break;

			case ' ':
			case '\t':
				// skip
				ptr++;
				break;

			case '+':
				if(!rpn_add_inst(vm, compiler, RPN_ADD, 0.f, 2, 1)) return 0;
				ptr++;
				break;
			case '-':
				if(!rpn_add_inst(vm, compiler, RPN_SUB, 0.f, 2, 1)) return 0;
				ptr++;
				break;
			case '*':
				if(!rpn_add_inst(vm, compiler, RPN_MUL, 0.f, 2, 1)) return 0;
				ptr++;
				break;
			case '/':
				if(!rpn_add_inst(vm, compiler, RPN_DIV, 0.f, 2, 1)) return 0;
				ptr++;
				break;
			case '%':
				if(!rpn_add_inst(vm, compiler, RPN_MOD, 0.f, 2, 1)) return 0;
				ptr++;
				break;
			case '^':
				if(!rpn_add_inst(vm, compiler, RPN_POW, 0.f, 2, 1)) return 0;
				ptr++;
				break;
			case '~':
				if(!rpn_add_inst(vm, compiler, RPN_NEG, 0.f, 1, 1)) return 0;
				ptr++;
				break;
			case '#':
				if(!rpn_add_inst(vm, compiler, RPN_XCHANGE, 0.f, 2, 2)) return 0;
				ptr++;
				break;
			case '@':
				switch(ptr[1])
				{
					case '@':
						if(!rpn_add_inst(vm, compiler, RPN_DUPL_TOP, 0.f, 0, 1)) return 0;
						ptr++;
						break;
					default:
						if(!rpn_add_inst(vm, compiler, RPN_DUPL_AT, 0.f, 1, 1)) return 0;
						break;
				}
				ptr++;
				break;

			case '!':
				switch(ptr[1])
				{
					case '=':
						if(!rpn_add_inst(vm, compiler, RPN_NOTEQ, 0.f, 2, 1)) return 0;
						ptr++;
						break;
					default:
						if(!rpn_add_inst(vm, compiler, RPN_NOT, 0.f, 1, 1)) return 0;
						break;
				}
				ptr++;
				break;
			case '?':
				if(!rpn_add_inst(vm, compiler, RPN_COND, 0.f, 3, 1)) return 0;
				ptr++;
				break;
			case '<':
				switch(ptr[1])
				{
					case '=':
						if(!rpn_add_inst(vm, compiler, RPN_LEQ, 0.f, 2, 1)) return 0;
						ptr++;
						break;
					case '<':
						if(!rpn_add_inst(vm, compiler, RPN_LSHIFT, 0.f, 2, 1)) return 0;
						ptr++;
						break;
					default:
						if(!rpn_add_inst(vm, compiler, RPN_LT, 0.f, 2, 1)) return 0;
						break;
				}
				ptr++;
				break;
			case '>':
				switch(ptr[1])
				{
					case '=':
						if(!rpn_add_inst(vm, compiler, RPN_GEQ, 0.f, 2, 1)) return 0;
						ptr++;
						break;
					case '>':
						if(!rpn_add_inst(vm, compiler, RPN_RSHIFT, 0.f, 2, 1)) return 0;
						ptr++;
						break;
					default:
						if(!rpn_add_inst(vm, compiler, RPN_GT, 0.f, 2, 1)) return 0;
						break;
				}
				ptr++;
				break;
			case '=':
				switch(ptr[1])
				{
					case '=':
						if(!rpn_add_inst(vm, compiler, RPN_EQ, 0.f, 2, 1)) return 0;
						ptr++;
						break;
					default:
						return 0; // parse error
				}
				ptr++;
				break;
			case '&':
				switch(ptr[1])
				{
					case '&':
						if(!rpn_add_inst(vm, compiler, RPN_LOGICAL_AND, 0.f, 2, 1)) return 0;
						ptr++;
						break;
					default:
						if(!rpn_add_inst(vm, compiler, RPN_BITWISE_AND, 0.f, 2, 1)) return 0;
						break;
				}
				ptr++;
				break;
			case '|':
				switch(ptr[1])
				{
					case '|':
						if(!rpn_add_inst(vm, compiler, RPN_LOGICAL_OR, 0.f, 2, 1)) return 0;
						ptr++;
						break;
					default:
						if(!rpn_add_inst(vm, compiler, RPN_BITWISE_OR, 0.f, 2, 1)) return 0;
						break;
				}
				ptr++;
				break;

			default:
			{
				char *endptr = NULL;
				float v = strtod(ptr, &endptr);
				if(ptr != endptr)
				{
					if(!rpn_add_inst(vm, compiler, RPN_PUSH_VALUE, v, 0, 1)) return 0;
					ptr = endptr;
				}
				else
					return 0; // parse error
			}
		}
	}

	return 1;
}

uint_fast8_t
rpn_compile(const char *args, Custom_Item *itm)
{
	RPN_VM *vm = &itm->vm;
	RPN_Compiler compiler = {
		.offset = 0,
		.pp = 0
	};

	const char *ptr = args;
	const char *end = args + strlen(args);
	uint_fast8_t counter = 0;

	// look for end of OSC path
	const char *path_end = strchr(ptr, ' ');
	if(!path_end)
		path_end = strchr(ptr, '\t');
	if(!path_end)
		path_end = strchr(ptr, '\0');
	if(!path_end)
		return 0; // parse error

	// copy and check OSC path
	size_t path_len = path_end + 1 - ptr;
	if(path_len > CUSTOM_PATH_LEN)
		return 0; // parse error
	strlcpy(itm->path, ptr, path_len);
	if(!osc_check_path(itm->path))
		return 0; // parse error

	// skip path
	ptr += path_len;

	while(ptr < end)
		switch(*ptr)
		{
			case ' ':
			case '\t':
				//skip white space
				ptr++;
				break;

			case OSC_INT32:
			{
				ptr++; // skip 'i'
				if(*ptr == '(')
				{
					ptr++; // skip '('
					char *closing = strchr(ptr, ')');
					if(closing)
					{
						size_t size = closing - ptr;
						if(rpn_compile_sub(ptr, size, vm, &compiler))
						{
							ptr += size;
							ptr++; // skip ')'

							if(!rpn_add_inst(vm, &compiler, RPN_POP_INT32, 0.f, 1, 0)) return 0;
							if(counter >= CUSTOM_FMT_LEN) return 0;
							itm->fmt[counter++] = OSC_INT32;
						}
						else
							return 0; // parse error
					}
					else
						return 0; // parse error
				}
				else
					return 0; // parse error
				break;
			}

			case OSC_FLOAT:
			{
				ptr++; // skip 'f'
				if(*ptr == '(')
				{
					ptr++; // skip '('
					char *closing = strchr(ptr, ')');
					if(closing)
					{
						size_t size = closing - ptr;
						if(rpn_compile_sub(ptr, size, vm, &compiler))
						{
							ptr += size;
							ptr++; // skip ')'

							if(!rpn_add_inst(vm, &compiler, RPN_POP_FLOAT, 0.f, 1, 0)) return 0;
							if(counter >= CUSTOM_FMT_LEN) return 0;
							itm->fmt[counter++] = OSC_FLOAT;
						}
						else
							return 0; // parse error
					}
					else
						return 0; // parse error
				}
				else
					return 0; // parse error
				break;
			}

			case OSC_MIDI:
			{
				ptr++; // skip 'm'
				if(*ptr == '(')
				{
					ptr++; // skip '('
					char *closing = strchr(ptr, ')');
					if(closing)
					{
						size_t size = closing - ptr;
						if(rpn_compile_sub(ptr, size, vm, &compiler))
						{
							ptr += size;
							ptr++; // skip ')'

							if(!rpn_add_inst(vm, &compiler, RPN_POP_MIDI, 0.f, 4, 0)) return 0;
							if(counter >= CUSTOM_FMT_LEN) return 0;
							itm->fmt[counter++] = OSC_MIDI;
						}
						else
							return 0; // parse error
					}
					else
						return 0; // parse error
				}
				else
					return 0; // parse error
				break;
			}

			case OSC_TRUE:
			case OSC_FALSE:
			case OSC_NIL:
			case OSC_BANG:
				if(counter >= CUSTOM_FMT_LEN) return 0;
				itm->fmt[counter++] = *ptr;
				break;

			//TODO OSC_STRING

			default:
				return 0; // parse error
		}

	if(!rpn_add_inst(vm, &compiler, RPN_TERMINATOR, 0.f, 0, 0)) return 0;
	if(counter >= CUSTOM_FMT_LEN) return 0;
	itm->fmt[counter++] = '\0';

	return (ptr == end) && (compiler.pp >= 0);
}

D dhcpc/dhcpc.c => dhcpc/dhcpc.c +0 -483
@@ 1,483 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#include <string.h>

#include "dhcpc_private.h"

#include <libmaple/systick.h>

#include <wiz.h>
#include <config.h>
#include <sntp.h>
#include <chimutil.h>
#include <arp.h>
#include <mdns-sd.h>

// global
DHCPC dhcpc = {
	.state = DISCOVER
};

static uint32_t xid = 0x3903F326; // TODO make this random or based on uid?
static const uint8_t dhcp_message_type_discover [1] = {DHCPDISCOVER};
static uint8_t client_identifier [7] = {1, 0, 0, 0, 0, 0, 0};
//static const uint8_t host_name [8] = {'c', 'h', 'i', 'm', 'a', 'e', 'r', 'a'};
static const uint8_t parameter_request_list [2] = {OPTION_SUBNET_MASK, OPTION_ROUTER};

static BOOTP_Option dhcp_discover_options [] = {
	BOOTP_OPTION(OPTION_DHCP_MESSAGE_TYPE, 1,(uint8_t *)dhcp_message_type_discover),
	BOOTP_OPTION(OPTION_CLIENT_IDENTIFIER, 7, client_identifier),
	//BOOTP_OPTION(OPTION_HOST_NAME, 8,(uint8_t *)host_name),
	BOOTP_OPTION(OPTION_PARAMETER_REQUEST_LIST, 2,(uint8_t *)parameter_request_list),
	BOOTP_OPTION_END
};

static const uint8_t dhcp_message_type_request [1] = {DHCPREQUEST};
static uint8_t dhcp_request_ip [4] = {0, 0, 0, 0};
static uint8_t dhcp_server_identifier [4] = {0, 0, 0, 0};

static BOOTP_Option dhcp_request_options [] = {
	BOOTP_OPTION(OPTION_DHCP_MESSAGE_TYPE, 1,(uint8_t *)dhcp_message_type_request),
	BOOTP_OPTION(OPTION_DHCP_REQUEST_IP, 4, dhcp_request_ip),
	BOOTP_OPTION(OPTION_DHCP_SERVER_IDENTIFIER, 4, dhcp_server_identifier),
	BOOTP_OPTION_END
};

static const uint8_t dhcp_message_type_decline [1] = {DHCPDECLINE};

static BOOTP_Option dhcp_decline_options [] = {
	BOOTP_OPTION(OPTION_DHCP_MESSAGE_TYPE, 1,(uint8_t *)dhcp_message_type_decline),
	BOOTP_OPTION(OPTION_DHCP_REQUEST_IP, 4,(uint8_t *)dhcp_request_ip),
	BOOTP_OPTION(OPTION_DHCP_SERVER_IDENTIFIER, 4, dhcp_server_identifier),
	BOOTP_OPTION_END
};

static DHCP_Packet_Packed dhcp_packet = {
	.bootp = {
		.op = BOOTP_OP_REQUEST,
		.htype = BOOTP_HTYPE_ETHERNET,
		.hlen = BOOTP_HLEN_ETHERNET,

		.flags = BOOTP_FLAGS_BROADCAST
	},

	.magic_cookie = DHCP_MAGIC_COOKIE,

	.options = dhcp_discover_options
};

static uint16_t
_dhcp_packet_serialize(DHCP_Packet_Packed *packet, uint8_t *buf)
{
	uint8_t *buf_ptr = buf;

	memcpy(buf_ptr, &packet->bootp, sizeof(BOOTP_Packet));
	buf_ptr += sizeof(BOOTP_Packet);

	memset(buf_ptr, 0, sizeof(BOOTP_Packet_Optionals));
	buf_ptr += sizeof(BOOTP_Packet_Optionals);

	memcpy(buf_ptr, &packet->magic_cookie, 4);
	buf_ptr += 4;

	BOOTP_Option *opt;
	for(opt=packet->options; opt->code!=255; opt++)
	{
		*buf_ptr++ = opt->code;
		*buf_ptr++ = opt->len;
		memcpy(buf_ptr, opt->dat, opt->len);
		buf_ptr += opt->len;
	}
	*buf_ptr++ = 255; // option end

	return buf_ptr-buf;
}

static uint16_t
dhcpc_discover(uint8_t *buf, uint16_t secs)
{
	dhcp_packet.bootp.xid = htonl(++xid);
	dhcp_packet.bootp.secs = hton(secs);

	memcpy(dhcp_packet.bootp.chaddr, config.comm.mac, 6);

	dhcp_packet.options = dhcp_discover_options;

	memcpy(&client_identifier[1], config.comm.mac, 6);

	dhcpc.state = OFFER;

	return _dhcp_packet_serialize(&dhcp_packet, buf);
}

static uint16_t
dhcpc_request(uint8_t *buf, uint16_t secs)
{
	dhcp_packet.bootp.xid = htonl(xid);
	dhcp_packet.bootp.secs = hton(secs);

	memcpy(dhcp_packet.bootp.siaddr, dhcpc.server_ip, 4);
	memcpy(dhcp_packet.bootp.chaddr, config.comm.mac, 6);

	dhcp_packet.options = dhcp_request_options;

	memcpy(dhcp_request_ip, dhcpc.ip, 4);
	memcpy(dhcp_server_identifier, dhcpc.server_ip, 4);

	dhcpc.state = ACK;

	return _dhcp_packet_serialize(&dhcp_packet, buf);
}

static uint16_t
dhcpc_decline(uint8_t *buf, uint16_t secs)
{
	dhcp_packet.bootp.xid = htonl(xid);
	dhcp_packet.bootp.secs = hton(secs);

	memcpy(dhcp_packet.bootp.siaddr, dhcpc.server_ip, 4);
	memcpy(dhcp_packet.bootp.chaddr, config.comm.mac, 6);

	dhcp_packet.options = dhcp_decline_options;

	memcpy(dhcp_request_ip, dhcpc.ip, 4);
	memcpy(dhcp_server_identifier, dhcpc.server_ip, 4);

	dhcpc.state = DISCOVER;

	return _dhcp_packet_serialize(&dhcp_packet, buf);
}

static void
dhcpc_dispatch(uint8_t *buf, uint16_t size)
{
	(void)size;
	DHCP_Packet_Unpacked *recv =(DHCP_Packet_Unpacked *)buf;

	// check for magic_cookie
	if(memcmp(recv->magic_cookie, dhcp_packet.magic_cookie, 4))
		return;

	recv->bootp.xid = htonl(recv->bootp.xid);
	recv->bootp.secs = hton(recv->bootp.secs);

	uint32_t *_ip =(uint32_t *)recv->bootp.yiaddr;
	if(*_ip != 0x00000000UL)
		memcpy(dhcpc.ip, recv->bootp.yiaddr, 4);

	uint8_t *buf_ptr = buf + sizeof(DHCP_Packet_Unpacked);
	uint8_t code;
	while( (code = buf_ptr[0]) != OPTION_END)
	{
		uint8_t len = buf_ptr[1];
		uint8_t *dat = &buf_ptr[2];
		switch(code)
		{
			case OPTION_SUBNET_MASK:
				memcpy(dhcpc.subnet_mask, dat, 4);
				break;

			case OPTION_ROUTER:
				memcpy(dhcpc.router_ip, dat, 4);
				break;

			case OPTION_DOMAIN_NAME_SERVER:
				// not used
				break;

			case OPTION_HOST_NAME:
				// not used
				break;

			case OPTION_DOMAIN_NAME:
				// not used
				break;

			case OPTION_DHCP_REQUEST_IP:
				memcpy(dhcpc.ip, dat, 4);
				break;

			case OPTION_IP_ADDRESS_LEASE_TIME:
				memcpy(&dhcpc.leastime, dat, 4);
				dhcpc.leastime = htonl(dhcpc.leastime) / 2; // refresh after half of it
				break;

			case OPTION_DHCP_MESSAGE_TYPE:
				switch(dat[0])
				{
					case DHCPOFFER:
						dhcpc.state = REQUEST;
						break;
					case DHCPACK:
						dhcpc.state = LEASE;
						break;
					case DHCPNAK:
						dhcpc.state = DISCOVER;
						break;
					default:
						// should never get here
						break;
				}
				break;

			case OPTION_DHCP_SERVER_IDENTIFIER:
				memcpy(dhcpc.server_ip, dat, 4);
				break;

			default:
				//ignore
				break;
		}
		buf_ptr += 2 + len;
	}
}

static void
dhcpc_cb(uint8_t *ip, uint16_t port, uint8_t *buf, uint16_t len)
{
	(void)ip;
	(void)port;
	dhcpc_dispatch(buf, len);
}

uint_fast8_t
dhcpc_claim(uint8_t *ip, uint8_t *gateway, uint8_t *subnet) //TODO migrate to ASIO
{
	uint8_t nil_ip [4] = {0, 0, 0, 0};
	uint8_t broadcast_ip [4] = {255, 255, 255, 255};

	// a DHCP claim is done in broadcast with IP: 0.0.0.0
	wiz_ip_set(nil_ip);
	udp_set_remote(config.dhcpc.socket.sock, broadcast_ip, config.dhcpc.socket.port[DST_PORT]);

	dhcpc.state = DISCOVER;
	dhcpc.delay = 4;
	dhcpc.timeout = systick_uptime() + dhcpc.delay*SNTP_SYSTICK_RATE;

	uint16_t secs;
	uint16_t len;
	while( (dhcpc.state != CLAIMED) && (dhcpc.state != TIMEOUT))
		switch(dhcpc.state)
		{
			case DISCOVER:
				secs = systick_uptime() / SNTP_SYSTICK_RATE + 1;
				len = dhcpc_discover(BUF_O_OFFSET(buf_o_ptr), secs);
				udp_send(config.dhcpc.socket.sock, BUF_O_BASE(buf_o_ptr), len);
				break;
			case OFFER:
				if(systick_uptime() > dhcpc.timeout) // timeout has occured, prepare to resend
				{
					dhcpc.state = DISCOVER;
					dhcpc.delay *= 2;
					dhcpc.timeout = systick_uptime() + dhcpc.delay*SNTP_SYSTICK_RATE;

					if(dhcpc.delay > 64) // maximal number of retries reached
						dhcpc.state = TIMEOUT;

					break;
				}

				udp_dispatch(config.dhcpc.socket.sock, BUF_I_BASE(buf_i_ptr), dhcpc_cb);

				if(dhcpc.state == REQUEST) // reset timeout for REQUEST
				{
					dhcpc.delay = 4;
					dhcpc.timeout = systick_uptime() + dhcpc.delay*SNTP_SYSTICK_RATE;
				}
				break;
			case REQUEST:
				secs = systick_uptime() / SNTP_SYSTICK_RATE + 1;
				len = dhcpc_request(BUF_O_OFFSET(buf_o_ptr), secs);
				udp_send(config.dhcpc.socket.sock, BUF_O_BASE(buf_o_ptr), len);
				break;
			case ACK:
				if(systick_uptime() > dhcpc.timeout) // timeout has occured, prepare to resend
				{
					dhcpc.state = REQUEST;
					dhcpc.delay *= 2;
					dhcpc.timeout = systick_uptime() + dhcpc.delay*SNTP_SYSTICK_RATE;

					if(dhcpc.delay > 64) // maximal number of retries reached
						dhcpc.state = TIMEOUT;

					break;
				}

				udp_dispatch(config.dhcpc.socket.sock, BUF_I_BASE(buf_i_ptr), dhcpc_cb);
				break;
			case DECLINE:
				//TODO needs to be tested
				secs = systick_uptime() / SNTP_SYSTICK_RATE + 1;
				len = dhcpc_decline(BUF_O_OFFSET(buf_o_ptr), secs);
				udp_send(config.dhcpc.socket.sock, BUF_O_BASE(buf_o_ptr), len);

				dhcpc.delay = 4;
				dhcpc.timeout = systick_uptime() + dhcpc.delay*SNTP_SYSTICK_RATE;
				break;
			case LEASE:
				dhcpc_enable(0);
				if(arp_probe(SOCK_ARP, dhcpc.ip)) // collision
				{
					dhcpc_enable(1); // ARP AND DHCP share the same socket
					// decline IP because of ARP collision
					dhcpc.state = DECLINE;
				}
				else
				{
					arp_announce(SOCK_ARP, dhcpc.ip);
					dhcpc_enable(1); // ARP AND DHCP share the same socket
					dhcpc.state = CLAIMED;
					memcpy(ip, dhcpc.ip, 4);
					memcpy(gateway, dhcpc.router_ip, 4);
					memcpy(subnet, dhcpc.subnet_mask, 4);

					// reconfigure lease timer and start it
					timer_pause(dhcpc_timer);
					dhcpc_timer_reconfigure();
					timer_resume(dhcpc_timer);

					//FIXME actually, the user should do this before enabling dhcpc, not?
					uint8_t brd [4];
					broadcast_address(brd, ip, subnet);
					memcpy(config.output.osc.socket.ip, brd, 4);
					//memcpy(config.config.osc.socket.ip, brd, 4); //FIXME remove
					memcpy(config.sntp.socket.ip, brd, 4);
					//memcpy(config.debug.osc.socket.ip, brd, 4); //FIXME remove
				}
				break;
			case TIMEOUT:
			case CLAIMED:
				// never reached
				break;
		}

	if(dhcpc.state == TIMEOUT)
		wiz_ip_set(config.comm.ip); // reset to current IP

	return dhcpc.state == CLAIMED;
}

uint_fast8_t //TODO get rid of duplicated code from dhcpc_claim
dhcpc_refresh()
{
	uint8_t *ip = dhcpc.ip;
	uint8_t *gateway = dhcpc.router_ip;
	uint8_t *subnet = dhcpc.subnet_mask;

	// a DHCP REFRESH is a DHCP REQUEST done in unicast
	udp_set_remote(config.dhcpc.socket.sock, dhcpc.server_ip, config.dhcpc.socket.port[DST_PORT]);

	dhcpc.state = REQUEST;
	dhcpc.delay = 4;
	dhcpc.timeout = systick_uptime() + dhcpc.delay*SNTP_SYSTICK_RATE;

	uint16_t secs;
	uint16_t len;
	while( (dhcpc.state != CLAIMED) && (dhcpc.state != TIMEOUT))
		switch(dhcpc.state)
		{
			case REQUEST:
				secs = systick_uptime() / SNTP_SYSTICK_RATE + 1;
				len = dhcpc_request(BUF_O_OFFSET(buf_o_ptr), secs);
				udp_send(config.dhcpc.socket.sock, BUF_O_BASE(buf_o_ptr), len);
				break;
			case ACK:
				if(systick_uptime() > dhcpc.timeout) // timeout has occured, prepare to resend
				{
					dhcpc.state = REQUEST;
					dhcpc.delay *= 2;
					dhcpc.timeout = systick_uptime() + dhcpc.delay*SNTP_SYSTICK_RATE;

					if(dhcpc.delay > 64) // maximal number of retries reached
						dhcpc.state = TIMEOUT;

					break;
				}

				udp_dispatch(config.dhcpc.socket.sock, BUF_I_BASE(buf_i_ptr), dhcpc_cb);
				break;
			case DECLINE:
				//TODO needs to be tested
				secs = systick_uptime() / SNTP_SYSTICK_RATE + 1;
				len = dhcpc_decline(BUF_O_OFFSET(buf_o_ptr), secs);
				udp_send(config.dhcpc.socket.sock, BUF_O_BASE(buf_o_ptr), len);

				dhcpc.delay = 4;
				dhcpc.timeout = systick_uptime() + dhcpc.delay*SNTP_SYSTICK_RATE;

				//TODO if refresh is declined or timed-out, claim a new IP after 7/8 of lease time
				break;
			case LEASE:
				dhcpc_enable(0);
				if(arp_probe(SOCK_ARP, dhcpc.ip)) // collision
				{
					dhcpc_enable(1); // ARP AND DHCP share the same socket
					// decline IP because of ARP collision
					dhcpc.state = DECLINE;
				}
				else
				{
					arp_announce(SOCK_ARP, dhcpc.ip);
					dhcpc_enable(1); // ARP AND DHCP share the same socket
					dhcpc.state = CLAIMED;
					memcpy(ip, dhcpc.ip, 4);
					memcpy(gateway, dhcpc.router_ip, 4);
					memcpy(subnet, dhcpc.subnet_mask, 4);

					// reconfigure lease timer and start it
					timer_pause(dhcpc_timer);
					dhcpc_timer_reconfigure();
					timer_resume(dhcpc_timer);
				}
				break;
			case DISCOVER:
			case OFFER:
			case TIMEOUT:
			case CLAIMED:
				// never reached
				break;
		}

	return dhcpc.state == CLAIMED;
}

/*
 * Config
 */

static uint_fast8_t
_dhcpc_enabled(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	uint8_t enabled = config.dhcpc.enabled;

	// needs a config save and reboot to take action
	uint_fast8_t ret = config_check_bool(path, fmt, argc, buf, &config.dhcpc.enabled);

	if(config.mdns.socket.enabled && (enabled != config.dhcpc.enabled) )
		mdns_update(); // announce new name

	return ret;
}

/*
 * Query
 */

const OSC_Query_Item dhcpc_tree [] = {
	OSC_QUERY_ITEM_METHOD("enabled", "Enable/disable", _dhcpc_enabled, config_boolean_args),
};

D dhcpc/dhcpc_private.h => dhcpc/dhcpc_private.h +0 -100
@@ 1,100 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _DHCPC_PRIVATE_H_
#define _DHCPC_PRIVATE_H_

#include <dhcpc.h>

#define DHCPDISCOVER 1
#define DHCPOFFER 2
#define DHCPREQUEST 3
#define DHCPDECLINE 4
#define DHCPACK 5
#define DHCPNAK 6
#define DHCPRELEASE 7

#define BOOTP_OP_REQUEST 1
#define BOOTP_HTYPE_ETHERNET 1
#define BOOTP_HLEN_ETHERNET 6
#define BOOTP_FLAGS_BROADCAST {0x80, 0x00}
#define BOOTP_FLAGS_UNICAST {0x00, 0x00}
#define DHCP_MAGIC_COOKIE {0x63, 0x82, 0x53, 0x63}

#define OPTION_SUBNET_MASK 1
#define OPTION_ROUTER 3
#define OPTION_DOMAIN_NAME_SERVER 6
#define OPTION_HOST_NAME 12
#define OPTION_DOMAIN_NAME 15
#define OPTION_DHCP_REQUEST_IP 50
#define OPTION_IP_ADDRESS_LEASE_TIME 51
#define OPTION_DHCP_MESSAGE_TYPE 53
#define OPTION_DHCP_SERVER_IDENTIFIER 54
#define OPTION_PARAMETER_REQUEST_LIST 55
#define OPTION_CLIENT_IDENTIFIER 61
#define OPTION_END 255

typedef struct _BOOTP_Packet BOOTP_Packet;
typedef struct _BOOTP_Packet_Optionals BOOTP_Packet_Optionals;
typedef struct _BOOTP_Option BOOTP_Option;
typedef struct _DHCP_Packet_Packed DHCP_Packet_Packed;
typedef struct _DHCP_Packet_Unpacked DHCP_Packet_Unpacked;

struct _BOOTP_Packet {
	uint8_t op;					// 1: request, 0: reply
	uint8_t htype;			// 1: ethernet
	uint8_t hlen;				// 6: MAC/ethernet
	uint8_t hops;				// optional

	uint32_t xid;				// session ID
	uint16_t secs;
	uint8_t flags [2];

	uint8_t ciaddr [4];	// client IP
	uint8_t yiaddr [4]; // own IP
	uint8_t siaddr [4]; // server IP
	uint8_t giaddr [4]; // relay agent IP
	uint8_t chaddr [16];// client MAC
} __attribute((packed));

struct _BOOTP_Packet_Optionals {
	char sname [64];		// server name [optional]
	char file [128];		// file name [optional]
} __attribute((packed));

struct _BOOTP_Option {
	uint8_t code;
	uint8_t len;
	uint8_t *dat;
} __attribute((packed));

#define BOOTP_OPTION(CODE,LEN,DAT) {.code=CODE,.len=LEN,.dat=DAT}
#define BOOTP_OPTION_END {.code=OPTION_END}

struct _DHCP_Packet_Packed {
	BOOTP_Packet bootp;
	uint8_t magic_cookie [4];
	BOOTP_Option *options;
} __attribute((packed));

struct _DHCP_Packet_Unpacked {
	BOOTP_Packet bootp;
	BOOTP_Packet_Optionals optionals;
	uint8_t magic_cookie [4];
} __attribute((packed));

#endif // _DHCPC_PRIVATE_H_

D dummy/dummy.c => dummy/dummy.c +0 -204
@@ 1,204 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#include <string.h>
#include <math.h> // floor

#include <chimaera.h>
#include <chimutil.h>
#include <config.h>
#include <cmc.h>

#include <dummy.h>

static const char *dummy_idle_str = "/idle";
static const char *dummy_on_str = "/on";
static const char *dummy_off_str ="/off";
static const char *dummy_set_str ="/set";

static const char *dummy_idle_fmt = "";
static const char *dummy_on_fmt = "iiiff";
static const char *dummy_off_fmt [2] = {
	[0] = "i",			// !redundancy
	[1] = "iii"			// redundancy
};
static const char *dummy_set_fmt [2][2] = {
	[0] = {					// !redundancy
		[0] = "iff",		// !derivatives
		[1] = "iffff" 	// derivatives
	},
	[1] = {					// redundancy
		[0] = "iiiff",	// !derivatives
		[1] = "iiiffff"	// derivatives
	}
};

static osc_data_t *pack;
static osc_data_t *bndl;

static osc_data_t *
dummy_engine_frame_cb(osc_data_t *buf, osc_data_t *end, CMC_Frame_Event *fev)
{
	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;

	if(cmc_engines_active + config.dump.enabled > 1)
		buf_ptr = osc_start_bundle_item(buf_ptr, end, &pack);
	buf_ptr = osc_start_bundle(buf_ptr, end, fev->offset, &bndl);

	if(!(fev->nblob_old + fev->nblob_new))
	{
		buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
		{
			buf_ptr = osc_set_path(buf_ptr, end, dummy_idle_str);
			buf_ptr = osc_set_fmt(buf_ptr, end, dummy_idle_fmt);
		}
		buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);
	}

	return buf_ptr;
}

static osc_data_t *
dummy_engine_end_cb(osc_data_t *buf, osc_data_t *end, CMC_Frame_Event *fev)
{
	(void)fev;
	osc_data_t *buf_ptr = buf;

	buf_ptr = osc_end_bundle(buf_ptr, end, bndl);
	if(cmc_engines_active + config.dump.enabled > 1)
		buf_ptr = osc_end_bundle_item(buf_ptr, end, pack);

	return buf_ptr;
}

static osc_data_t *
dummy_engine_on_cb(osc_data_t *buf, osc_data_t *end, CMC_Blob_Event *bev)
{
	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;

	buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
	{
		buf_ptr = osc_set_path(buf_ptr, end, dummy_on_str);
		buf_ptr = osc_set_fmt(buf_ptr, end, dummy_on_fmt);
		buf_ptr = osc_set_int32(buf_ptr, end, bev->sid);
		buf_ptr = osc_set_int32(buf_ptr, end, bev->gid);
		buf_ptr = osc_set_int32(buf_ptr, end, bev->pid);
		buf_ptr = osc_set_float(buf_ptr, end, bev->x);
		buf_ptr = osc_set_float(buf_ptr, end, bev->y);
	}
	buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);

	return buf_ptr;
}

static osc_data_t *
dummy_engine_off_cb(osc_data_t *buf, osc_data_t *end, CMC_Blob_Event *bev)
{
	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;
	uint_fast8_t redundancy = config.dummy.redundancy;

	buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
	{
		buf_ptr = osc_set_path(buf_ptr, end, dummy_off_str);
		buf_ptr = osc_set_fmt(buf_ptr, end, dummy_off_fmt[redundancy]);
		buf_ptr = osc_set_int32(buf_ptr, end, bev->sid);
		if(redundancy)
		{
			buf_ptr = osc_set_int32(buf_ptr, end, bev->gid);
			buf_ptr = osc_set_int32(buf_ptr, end, bev->pid);
		}
	}
	buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);

	return buf_ptr;
}

static osc_data_t *
dummy_engine_set_cb(osc_data_t *buf, osc_data_t *end, CMC_Blob_Event *bev)
{
	osc_data_t *buf_ptr = buf;
	osc_data_t *itm;
	uint_fast8_t redundancy = config.dummy.redundancy;
	uint_fast8_t derivatives = config.dummy.derivatives;

	buf_ptr = osc_start_bundle_item(buf_ptr, end, &itm);
	{
		buf_ptr = osc_set_path(buf_ptr, end, dummy_set_str);
		buf_ptr = osc_set_fmt(buf_ptr, end, dummy_set_fmt[redundancy][derivatives]);
		buf_ptr = osc_set_int32(buf_ptr, end, bev->sid);
		if(redundancy)
		{
			buf_ptr = osc_set_int32(buf_ptr, end, bev->gid);
			buf_ptr = osc_set_int32(buf_ptr, end, bev->pid);
		}
		buf_ptr = osc_set_float(buf_ptr, end, bev->x);
		buf_ptr = osc_set_float(buf_ptr, end, bev->y);
		if(derivatives)
		{
			buf_ptr = osc_set_float(buf_ptr, end, bev->vx);
			buf_ptr = osc_set_float(buf_ptr, end, bev->vy);
		}
	}
	buf_ptr = osc_end_bundle_item(buf_ptr, end, itm);

	return buf_ptr;
}

CMC_Engine dummy_engine = {
	NULL,
	dummy_engine_frame_cb,
	dummy_engine_on_cb,
	dummy_engine_off_cb,
	dummy_engine_set_cb,
	dummy_engine_end_cb
};

/*
 * Config
 */
static uint_fast8_t
_dummy_enabled(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	uint_fast8_t res = config_check_bool(path, fmt, argc, buf, &config.dummy.enabled);
	cmc_engines_update();
	return res;
}

static uint_fast8_t
_dummy_redundancy(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return config_check_bool(path, fmt, argc, buf, &config.dummy.redundancy);
}

static uint_fast8_t
_dummy_derivatives(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return config_check_bool(path, fmt, argc, buf, &config.dummy.derivatives);
}

/*
 * Query
 */

const OSC_Query_Item dummy_tree [] = {
	OSC_QUERY_ITEM_METHOD("enabled", "Enable/disable", _dummy_enabled, config_boolean_args),
	OSC_QUERY_ITEM_METHOD("redundancy", "Send redundant data", _dummy_redundancy, config_boolean_args),
	OSC_QUERY_ITEM_METHOD("derivatives", "Calculate derivatives", _dummy_derivatives, config_boolean_args)
};

D engines/custom.h => engines/custom.h +0 -110
@@ 1,110 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _CUSTOM_H_
#define _CUSTOM_H_

#include <cmc.h>
#include <oscquery.h>

#define CUSTOM_MAX_EXPR		8
#define CUSTOM_MAX_INST		32

#define CUSTOM_PATH_LEN		64
#define CUSTOM_FMT_LEN		12
#define CUSTOM_ARGS_LEN		128

typedef struct _Custom_Item  Custom_Item;
typedef enum _RPN_Instruction RPN_Instruction;
typedef enum _RPN_Destination RPN_Destination;
typedef struct _RPN_VM RPN_VM;

enum _RPN_Instruction {
	RPN_TERMINATOR = 0,

	RPN_PUSH_VALUE,
	RPN_POP_INT32,
	RPN_POP_FLOAT,
	RPN_POP_MIDI,

	RPN_PUSH_FID,
	RPN_PUSH_SID,
	RPN_PUSH_GID,
	RPN_PUSH_PID,
	RPN_PUSH_X,
	RPN_PUSH_Z,
	RPN_PUSH_VX,
	RPN_PUSH_VZ,
	RPN_PUSH_N,

	RPN_PUSH_REG,
	RPN_POP_REG,

	RPN_ADD,
	RPN_SUB,
	RPN_MUL,
	RPN_DIV,
	RPN_MOD,
	RPN_POW,
	RPN_NEG,
	RPN_XCHANGE,
	RPN_DUPL_AT,
	RPN_DUPL_TOP,
	RPN_LSHIFT,
	RPN_RSHIFT,

	RPN_LOGICAL_AND,
	RPN_BITWISE_AND,
	RPN_LOGICAL_OR,
	RPN_BITWISE_OR,

	RPN_NOT,
	RPN_NOTEQ,
	RPN_COND,
	RPN_LT,
	RPN_LEQ,
	RPN_GT,
	RPN_GEQ,
	RPN_EQ
};

enum _RPN_Destination {
	RPN_NONE = 0,
	RPN_FRAME,
	RPN_ON,
	RPN_OFF,
	RPN_SET,
	RPN_END,
	RPN_IDLE
};

struct _RPN_VM {
	RPN_Instruction inst [CUSTOM_MAX_INST];
	float val [CUSTOM_MAX_INST];
};

struct _Custom_Item {
	RPN_Destination dest;
	char path [CUSTOM_PATH_LEN];
	char fmt [CUSTOM_FMT_LEN];
	RPN_VM vm;
};

extern CMC_Engine custom_engine;
extern const OSC_Query_Item custom_tree [3];

#endif // _CUSTOM_H_

D engines/dummy.h => engines/dummy.h +0 -27
@@ 1,27 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _DUMMY_H_
#define _DUMMY_H_

#include <cmc.h>
#include <oscquery.h>

extern CMC_Engine dummy_engine;
extern const OSC_Query_Item dummy_tree [3];

#endif // _DUMMY_H_

D engines/scsynth.h => engines/scsynth.h +0 -48
@@ 1,48 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _SCSYNTH_H_
#define _SCSYNTH_H_

#include <cmc.h>
#include <oscquery.h>

typedef enum _SCSynth_Add_Action SCSynth_Add_Action;
typedef struct _SCSynth_Group SCSynth_Group;

enum _SCSynth_Add_Action {
	SCSYNTH_ADD_TO_HEAD = 0,
	SCSYNTH_ADD_TO_TAIL = 1
};

struct _SCSynth_Group {
	char name[8];
	uint16_t sid;
	uint16_t group;
	uint16_t out;
	uint8_t arg;
	uint8_t alloc;
	uint8_t gate;
	uint8_t add_action;
	uint8_t is_group;
};

extern SCSynth_Group *scsynth_groups;
extern CMC_Engine scsynth_engine;
extern const OSC_Query_Item scsynth_tree [4];

#endif // _SCSYNTH_H_

D engines/tuio1.h => engines/tuio1.h +0 -27
@@ 1,27 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _TUIO1_H_
#define _TUIO1_H_

#include <cmc.h>
#include <oscquery.h>

extern CMC_Engine tuio1_engine;
extern const OSC_Query_Item tuio1_tree [2];

#endif // _TUIO1_H_

M firmware.c => firmware.c +4 -304
@@ 43,14 43,8 @@
#include <chimutil.h>
#include <debug.h>
#include <eeprom.h>
#include <ptp.h>
#include <sntp.h>
#include <config.h>
#include <tube.h>
#include <ipv4ll.h>
#include <mdns-sd.h>
#include <dhcpc.h>
#include <arp.h>
#include <engines.h>
#include <wiz.h>
#include <calibration.h>


@@ 94,24 88,14 @@ static volatile uint_fast8_t adc3_dma_err = 0;
static volatile uint_fast8_t adc_time_up = 1;
static volatile uint_fast8_t adc_raw_ptr = 1;
static volatile uint_fast8_t mux_counter = MUX_MAX;
static volatile uint_fast8_t sync_should_request = 1; // send first request at boot
static volatile uint_fast8_t sntp_should_listen = 0;
static volatile uint_fast8_t ptp_should_request = 0;
static volatile uint_fast8_t ptp_event_should_listen = 0;
static volatile uint_fast8_t ptp_general_should_listen = 0;
static volatile uint_fast8_t mdns_should_listen = 0;
static volatile uint_fast8_t output_should_listen = 0;
static volatile uint_fast8_t config_should_listen = 0;
static volatile uint_fast8_t debug_should_listen = 0;
static volatile uint_fast8_t dhcpc_should_listen = 0;
static volatile uint_fast8_t dhcpc_needs_refresh = 0;
static volatile uint_fast8_t mdns_timeout = 0;
static volatile uint_fast8_t wiz_needs_attention = 0;
static volatile int64_t wiz_ptp_tick;

Reset_Mode reset_mode;

static OSC_Timetag now;
static OSC_Timetag now = 0;

static void __CCM_TEXT__
adc_timer_irq(void)


@@ 120,25 104,6 @@ adc_timer_irq(void)
	timer_pause(adc_timer);
}

static void __CCM_TEXT__
sync_timer_irq(void)
{
	sync_should_request = 1;
}

static void __CCM_TEXT__
dhcpc_timer_irq(void)
{
	dhcpc_needs_refresh = 1;
}

static void __CCM_TEXT__
mdns_timer_irq(void)
{
	mdns_timeout = 1;
	timer_pause(mdns_timer);
}

static void
soft_irq(void)
{


@@ 151,7 116,6 @@ static void __CCM_TEXT__
wiz_irq(void)
{
	//TODO substract 12 cycles interrupt latency for ARM Cortex M4?
	wiz_ptp_tick = ptp_uptime();
	wiz_needs_attention = 1;
}



@@ 173,45 137,6 @@ wiz_debug_irq(uint8_t isr)
	debug_should_listen = isr;
}

static void __CCM_TEXT__
wiz_mdns_irq(uint8_t isr)
{
	mdns_should_listen = isr;
}

static void __CCM_TEXT__
wiz_sntp_irq(uint8_t isr)
{
	sntp_should_listen = isr;
}

static void __CCM_TEXT__
ptp_timer_irq(void)
{
	ptp_should_request = 1;
	timer_pause(ptp_timer);
}

static void __CCM_TEXT__
wiz_ptp_event_irq(uint8_t isr)
{
	ptp_event_should_listen = isr;
}

static void __CCM_TEXT__
wiz_ptp_general_irq(uint8_t isr)
{
	ptp_general_should_listen = isr;
}

#if 0
static void __CCM_TEXT__
wiz_dhcpc_irq(uint8_t isr)
{
	dhcpc_should_listen = isr;
}
#endif

static inline __always_inline void
_counter_inc(void)
{


@@ 383,33 308,6 @@ config_cb(uint8_t *ip, uint16_t port, uint8_t *buf, uint16_t len)
		DEBUG("s", "invalid OSC packet");
}

static void __CCM_TEXT__
sntp_cb(uint8_t *ip, uint16_t port, uint8_t *buf, uint16_t len)
{
	(void)ip;
	(void)port;
	(void)len;
	sntp_timestamp_refresh(wiz_ptp_tick, &now, NULL);
	sntp_dispatch(buf, now);
}

static void __CCM_TEXT__
ptp_cb(uint8_t *ip, uint16_t port, uint8_t *buf, uint16_t len)
{
	(void)ip;
	(void)port;
	(void)len;
	ptp_dispatch(buf, wiz_ptp_tick);
}

static void //__CCM_TEXT__
mdns_cb(uint8_t *ip, uint16_t port, uint8_t *buf, uint16_t len)
{
	(void)ip;
	(void)port;
	mdns_dispatch(buf, len);
}

// loops are explicitely unrolled which makes it fast but cumbersome to read
static void __CCM_TEXT__
adc_fill(uint_fast8_t raw_ptr)


@@ 560,10 458,9 @@ loop(void)
	uint_fast8_t cmc_stat;
	uint_fast8_t cmc_job = 0;
	uint_fast16_t cmc_len = 0;
	uint_fast16_t len = 0;

	uint_fast8_t first = 1;
	OSC_Timetag offset;
	OSC_Timetag offset = OSC_IMMEDIATE;

//#define OSCTEST
#ifdef OSCTEST


@@ 648,13 545,6 @@ loop(void)
#endif
			adc_fill(adc_raw_ptr);

			if(config.sntp.socket.enabled)
				sntp_timestamp_refresh(ptp_uptime(), &now, &offset);
			else if(config.ptp.event.enabled)
				ptp_timestamp_refresh(ptp_uptime(), &now, &offset);
			else // neither sNTP nor PTP active
				sntp_timestamp_refresh(ptp_uptime(), &now, &offset);

			// initiate OSC bundle
			osc_data_t *buf = BUF_O_OFFSET(buf_o_ptr);
			osc_data_t *end = BUF_O_MAX(buf_o_ptr);


@@ 794,95 684,6 @@ loop(void)
			debug_should_listen = 0;
		}
		
		// run sntp client
		if(config.sntp.socket.enabled)
		{
			if(sntp_should_listen & WIZ_Sn_IR_TIMEOUT)
			{
				sntp_enable(0);
				debug_str("sntp ARPto");
			}
			// listen for sntp request answer
			else if(sntp_should_listen & WIZ_Sn_IR_RECV)
			{
				udp_dispatch(config.sntp.socket.sock, BUF_I_BASE(buf_i_ptr), sntp_cb);
				sntp_should_listen = 0;
			}

			// send sntp request
			if(sync_should_request)
			{
				sntp_timestamp_refresh(ptp_uptime(), &now, NULL);
				len = sntp_request(BUF_O_OFFSET(buf_o_ptr), now);
				udp_send(config.sntp.socket.sock, BUF_O_BASE(buf_o_ptr), len);
				sync_should_request = 0;
			}
		}

		// run ptp client
		if(config.ptp.event.enabled)
		{
			if(ptp_event_should_listen & WIZ_Sn_IR_RECV)
			{
				udp_dispatch(config.ptp.event.sock, BUF_I_BASE(buf_i_ptr), ptp_cb);
				ptp_event_should_listen = 0;
			}

			if(ptp_general_should_listen & WIZ_Sn_IR_RECV)
			{
				udp_dispatch(config.ptp.general.sock, BUF_I_BASE(buf_i_ptr), ptp_cb);
				ptp_general_should_listen = 0;
			}

			if(ptp_should_request)
			{
				ptp_request();
				ptp_should_request = 0;
			}
		}

		// run ZEROCONF server
		if(config.mdns.socket.enabled)
		{
			// ARPto does not exist for multicast connections
			if(mdns_should_listen & WIZ_Sn_IR_RECV)
			{
				udp_dispatch(config.mdns.socket.sock, BUF_I_BASE(buf_i_ptr), mdns_cb);
				mdns_should_listen = 0;
			}

			if(mdns_timeout)
			{
				mdns_resolve_timeout();
				mdns_timeout = 0;
			}
		}

		// DHCPC REFRESH
		if(dhcpc_needs_refresh)
		{
			timer_pause(dhcpc_timer);
			dhcpc_enable(1);
			dhcpc_refresh();
			dhcpc_enable(0);
			dhcpc_needs_refresh = 0;
		}

		//FIXME asio
		/*
		if(config.dhcpc.socket.enabled && dhcpc_should_listen)
		{
			if(dhcpc_should_listen & WIZ_Sn_IR_TIMEOUT)
			{
				dhcpc_enable(0);
				debug_str("dhcpc ARPto");
			}
			else if(dhcpc_should_listen & WIZ_Sn_IR_RECV)
				udp_ignore(config.dhcpc.socket.sock);
			dhcpc_should_listen = 0;
		}
		*/

		adc_dma_block();

		if(config.sensors.rate)


@@ 909,74 710,6 @@ adc_timer_reconfigure(void)
	nvic_irq_set_priority(NVIC_ADC_TIMER, ADC_TIMER_PRIORITY);
}

void 
sync_timer_reconfigure(void)
{
	uint16_t prescaler = 0xffff; 
	uint16_t reload = 72e6 / 0xffff * config.sntp.tau;
	uint16_t compare = reload;

	timer_set_prescaler(sync_timer, prescaler);
	timer_set_reload(sync_timer, reload);
	timer_set_mode(sync_timer, TIMER_CH1, TIMER_OUTPUT_COMPARE);
	timer_set_compare(sync_timer, TIMER_CH1, compare);
	timer_attach_interrupt(sync_timer, TIMER_CH1, sync_timer_irq);
	timer_generate_update(sync_timer);

	nvic_irq_set_priority(NVIC_SYNC_TIMER, SYNC_TIMER_PRIORITY);
}

void 
ptp_timer_reconfigure(float sec)
{
	uint16_t prescaler = 0xffff; 
	uint16_t reload = 72e6 / 0xffff * sec;
	uint16_t compare = reload;

	timer_set_prescaler(ptp_timer, prescaler);
	timer_set_reload(ptp_timer, reload);
	timer_set_mode(ptp_timer, TIMER_CH1, TIMER_OUTPUT_COMPARE);
	timer_set_compare(ptp_timer, TIMER_CH1, compare);
	timer_attach_interrupt(ptp_timer, TIMER_CH1, ptp_timer_irq);
	timer_generate_update(ptp_timer);

	nvic_irq_set_priority(NVIC_PTP_TIMER, SYNC_TIMER_PRIORITY);
}

void 
dhcpc_timer_reconfigure(void)
{
	uint16_t prescaler = 0xffff; 
	uint16_t reload = 72e6 / 0xffff * dhcpc.leastime;
	uint16_t compare = reload;

	timer_set_prescaler(dhcpc_timer, prescaler);
	timer_set_reload(dhcpc_timer, reload);
	timer_set_mode(dhcpc_timer, TIMER_CH1, TIMER_OUTPUT_COMPARE);
	timer_set_compare(dhcpc_timer, TIMER_CH1, compare);
	timer_attach_interrupt(dhcpc_timer, TIMER_CH1, dhcpc_timer_irq);
	timer_generate_update(dhcpc_timer);

	nvic_irq_set_priority(NVIC_DHCP_TIMER, DHCPC_TIMER_PRIORITY);
}

void 
mdns_timer_reconfigure(void)
{
	uint16_t prescaler = 0xffff; 
	uint16_t reload = 72e6 / 0xffff * 2; // timeout after 2 seconds
	uint16_t compare = reload;

	timer_set_prescaler(mdns_timer, prescaler);
	timer_set_reload(mdns_timer, reload);
	timer_set_mode(mdns_timer, TIMER_CH1, TIMER_OUTPUT_COMPARE);
	timer_set_compare(mdns_timer, TIMER_CH1, compare);
	timer_attach_interrupt(mdns_timer, TIMER_CH1, mdns_timer_irq);
	timer_generate_update(mdns_timer);

	nvic_irq_set_priority(NVIC_MDNS_TIMER, MDNS_TIMER_PRIORITY);
}

void setup(void);
void
setup(void)


@@ 1067,7 800,7 @@ setup(void)

	// systick 
	systick_disable();
	systick_init(SNTP_SYSTICK_RELOAD_VAL);
	// XXX XXX XXX systick_init(SNTP_SYSTICK_RELOAD_VAL);

	nvic_irq_set_priority(NVIC_SYSTICK, 0x0); // needs highest priority
	nvic_irq_set_priority((exti_num)(PIN_MAP[UDP_INT].gpio_bit), 0x1); // needs second highest priority


@@ 1132,60 865,27 @@ setup(void)
	wiz_sockets_set(tx_mem, rx_mem);
	wiz_mac_set(config.comm.mac);
	
	// choose DHCP, IPv4LL or static IP
	uint_fast8_t claimed = 0;
	if(config.dhcpc.enabled)
	{
		dhcpc_enable(1);
		claimed = dhcpc_claim(config.comm.ip, config.comm.gateway, config.comm.subnet);
		dhcpc_enable(0); // disable socket again
	}
	if(!claimed && config.ipv4ll.enabled)
		IPv4LL_claim(config.comm.ip, config.comm.gateway, config.comm.subnet);

	// choose static IP
	wiz_comm_set(config.comm.mac, config.comm.ip, config.comm.gateway, config.comm.subnet);

	//TODO put this into config_enable?
	const uint8_t wiz_udp_multicast_irq_mask = WIZ_Sn_IR_RECV;
	const uint8_t wiz_udp_irq_mask = wiz_udp_multicast_irq_mask | WIZ_Sn_IR_TIMEOUT;
	const uint8_t wiz_tcp_irq_mask = wiz_udp_irq_mask | WIZ_Sn_IR_CON | WIZ_Sn_IR_DISCON;
	wiz_socket_irq_set(SOCK_SNTP, wiz_sntp_irq, wiz_udp_irq_mask);
	wiz_socket_irq_set(SOCK_PTP_EV, wiz_ptp_event_irq, wiz_udp_multicast_irq_mask);
	wiz_socket_irq_set(SOCK_PTP_GE, wiz_ptp_general_irq, wiz_udp_multicast_irq_mask);
	wiz_socket_irq_set(SOCK_OUTPUT, wiz_output_irq, wiz_tcp_irq_mask);
	wiz_socket_irq_set(SOCK_CONFIG, wiz_config_irq, wiz_tcp_irq_mask);
	wiz_socket_irq_set(SOCK_DEBUG, wiz_debug_irq, wiz_tcp_irq_mask);
	wiz_socket_irq_set(SOCK_MDNS, wiz_mdns_irq, wiz_udp_multicast_irq_mask);
	//wiz_socket_irq_set(SOCK_DHCPC, wiz_dhcpc_irq, wiz_udp_irq_mask); FIXME asio
	wiz_socket_irq_unset(SOCK_DHCPC); // = SOCK_ARP

	// initialize timers TODO move up
	timer_init(adc_timer);
	timer_pause(adc_timer);
	adc_timer_reconfigure();

	timer_init(sync_timer);

	timer_init(dhcpc_timer);
	timer_pause(dhcpc_timer);

	timer_init(mdns_timer);
	timer_pause(mdns_timer);
	
	timer_init(ptp_timer);
	timer_pause(ptp_timer);

	// initialize sockets
	output_enable(config.output.osc.socket.enabled);
	config_enable(config.config.osc.socket.enabled);
	sntp_enable(config.sntp.socket.enabled);
	ptp_enable(config.ptp.event.enabled);
	debug_enable(config.debug.osc.socket.enabled);
	mdns_enable(config.mdns.socket.enabled);
	
	if(config.mdns.socket.enabled)
		mdns_announce(); // announce new IP

	// set up ADCs
#if(ADC_DUAL_LENGTH > 0)
	adc_disable(ADC1);

D include/arp.h => include/arp.h +0 -35
@@ 1,35 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _ARP_H_
#define _ARP_H_

#include <stdint.h>

/*
 * send an ARP probe: investigate whether another hardware uses ip,
 * e.g. whether there is an IP collision.
 * returns 1 on collision, 0 otherwise
 */
uint_fast8_t arp_probe(uint8_t sock, uint8_t *ip);

/*
 * announce ip via ARP
 */
void arp_announce(uint8_t sock, uint8_t *ip);

#endif // _ARP_H_

M include/chimaera.h => include/chimaera.h +0 -15
@@ 94,10 94,7 @@ extern const stm32_pin_info PIN_MAP [];
#define SPI_RX_DMA_PRIORITY 0x3
#define SPI_TX_DMA_PRIORITY 0x4
#define ADC_TIMER_PRIORITY 0x5
#define SYNC_TIMER_PRIORITY 0x6
#define CONFIG_TIMER_PRIORITY 0x7
#define DHCPC_TIMER_PRIORITY 0x8
#define MDNS_TIMER_PRIORITY 0x9

#define BOOT_MODE_REG 0xF



@@ 201,18 198,6 @@ extern uint8_t buf_i[1] [CHIMAERA_BUFSIZE]; // general purpose input buffer
#define adc_timer TIMER1
#define NVIC_ADC_TIMER	NVIC_TIMER1_CC

#define sync_timer TIMER2
#define NVIC_SYNC_TIMER NVIC_TIMER2

#define ptp_timer TIMER3
#define NVIC_PTP_TIMER NVIC_TIMER3

#define mdns_timer TIMER4
#define NVIC_MDNS_TIMER NVIC_TIMER4

#define dhcpc_timer TIMER15
#define NVIC_DHCP_TIMER NVIC_TIMER1_BRK_TIMER15

void cpp_setup(void);

#endif // _CHIMIAERA_H_

M include/chimutil.h => include/chimutil.h +0 -8
@@ 28,18 28,10 @@ uint8_t subnet_to_cidr(uint8_t *subnet);
void broadcast_address(uint8_t *brd, uint8_t *ip, uint8_t *subnet);

void adc_timer_reconfigure(void);
void sync_timer_reconfigure(void);
void dhcpc_timer_reconfigure(void);
void mdns_timer_reconfigure(void);
void ptp_timer_reconfigure(float sec);

void output_enable(uint8_t b);
void config_enable(uint8_t b);
void sntp_enable(uint8_t b);
void ptp_enable(uint8_t b);
void debug_enable(uint8_t b);
void mdns_enable(uint8_t b);
void dhcpc_enable(uint8_t b);

typedef struct _Stop_Watch Stop_Watch;


M include/config.h => include/config.h +0 -51
@@ 23,8 23,6 @@
#include <cmc.h>
#include <chimaera.h>

#include <scsynth.h>
#include <custom.h>
#include <oscmidi.h>

#define SRC_PORT 0


@@ 106,16 104,6 @@ struct _Config {
		uint8_t derivatives;
	} tuio2;

	struct _tuio1 {
		uint8_t enabled;
		uint8_t custom_profile;
	} tuio1;

	struct _scsynth {
		uint8_t enabled;
		uint8_t derivatives;
	} scsynth;

	struct _oscmidi {
		uint8_t enabled;
		uint8_t multi;


@@ 124,20 112,8 @@ struct _Config {
		char path [64];
	} oscmidi;

	struct _dummy {
		uint8_t enabled;
		uint8_t redundancy;
		uint8_t derivatives;
	} dummy;

	struct _custom {
		uint8_t enabled;
		Custom_Item items [CUSTOM_MAX_EXPR];
	} custom;

	struct _output {
		OSC_Config osc;
		OSC_Timetag offset;
		struct {
			uint8_t x;
			uint8_t z;


@@ 149,36 125,10 @@ struct _Config {
		OSC_Config osc;
	} config;

	struct _ptp {
		uint8_t multiplier;
		uint8_t offset_stiffness;
		uint8_t delay_stiffness;
		Socket_Config event;
		Socket_Config general;
	} ptp;

	struct _sntp {
		uint8_t tau;
		Socket_Config socket;
	} sntp;

	struct _debug {
		OSC_Config osc;
	} debug;

	struct _ipv4ll {
		uint8_t enabled;
	} ipv4ll;

	struct _mdns {
		Socket_Config socket;
	} mdns;

	struct _dhcpc {
		uint8_t enabled;
		Socket_Config socket;
	} dhcpc;

	struct _sensors {
		uint8_t movingaverage_bitshift;
		uint8_t interpolation_mode;


@@ 187,7 137,6 @@ struct _Config {
	} sensors;

	CMC_Group groups [GROUP_MAX];
	SCSynth_Group scsynth_groups [GROUP_MAX];
	OSC_MIDI_Group oscmidi_groups [GROUP_MAX];
};


D include/dhcpc.h => include/dhcpc.h +0 -47
@@ 1,47 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _DHCPC_H_
#define _DHCPC_H_

#include <oscquery.h>

typedef enum _DHCPC_State {
	DISCOVER, OFFER, REQUEST, ACK, LEASE, TIMEOUT, CLAIMED, DECLINE
} DHCPC_State;

typedef struct _DHCPC DHCPC;

struct _DHCPC {
	DHCPC_State state;
	uint8_t delay;
	uint32_t timeout;
	uint32_t leastime;

	uint8_t ip [4];
	uint8_t server_ip [4];
	uint8_t router_ip [4];
	uint8_t subnet_mask [4];
};

uint_fast8_t dhcpc_claim(uint8_t *ip, uint8_t *gateway, uint8_t *subnet);
uint_fast8_t dhcpc_refresh(void);

extern DHCPC dhcpc;
extern const OSC_Query_Item dhcpc_tree [1];

#endif // _DHCPC_H_

M include/engines.h => include/engines.h +0 -4
@@ 20,10 20,6 @@

#include <dump.h>
#include <tuio2.h>
#include <tuio1.h>
#include <dummy.h>
#include <custom.h>
#include <oscmidi.h>
#include <scsynth.h>

#endif // _ENGINES_H_

D include/ipv4ll.h => include/ipv4ll.h +0 -29
@@ 1,29 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _IPV4LL_H_
#define _IPV4LL_H_

#include <stdint.h>

#include <oscquery.h>

extern const OSC_Query_Item ipv4ll_tree [1];

void IPv4LL_claim(uint8_t *ip, uint8_t *gateway, uint8_t *subnet);

#endif // _IPV4LL_H_

D include/mdns-sd.h => include/mdns-sd.h +0 -39
@@ 1,39 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _MDNS_H_
#define _MDNS_H_

#include <stdint.h>

#include <oscquery.h>

extern const OSC_Query_Item mdns_tree [1];

typedef void(*mDNS_Resolve_Cb)(uint8_t *ip, void *data);

void mdns_dispatch(uint8_t *buf, uint16_t len);

void mdns_announce(void);
void mdns_update(void);
void mdns_goodbye(void);

//TODO allow multiple concurrent resolvings
void mdns_resolve_timeout(void);
uint_fast8_t mdns_resolve(const char *name, mDNS_Resolve_Cb cb, void *data);

#endif // _MDNS_H_

D include/ptp.h => include/ptp.h +0 -34
@@ 1,34 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _PTP_H_
#define _PTP_H_

#include <stdint.h>

#include <netdef.h>
#include <oscquery.h>

extern const OSC_Query_Item ptp_tree [6];

void ptp_reset(void);
int64_t ptp_uptime(void);
void ptp_timestamp_refresh(int64_t tick, OSC_Timetag *now, OSC_Timetag *offset);
void ptp_request(void);
void ptp_dispatch(uint8_t *buf, int64_t tick);

#endif // _PTP_H_

M include/sensors.h => include/sensors.h +1 -3
@@ 30,9 30,7 @@ extern const OSC_Query_Item sensors_tree [6];

enum Interpolation_Mode {
	INTERPOLATION_NONE,
	INTERPOLATION_QUADRATIC,
	INTERPOLATION_CATMULL,
	INTERPOLATION_LAGRANGE
	INTERPOLATION_QUADRATIC
};

#endif // _SENSORS_H_

D include/sntp.h => include/sntp.h +0 -47
@@ 1,47 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _SNTP_H_
#define _SNTP_H_

#include <stdint.h>

#include <oscquery.h>

/*
#define SNTP_SYSTICK_RELOAD_VAL 719 // 10us
#define SNTP_SYSTICK_DURATION 0.00001ULLK // 10us
#define SNTP_SYSTICK_RATE 100000
#define SNTP_SYSTICK_US 10
*/

#define SNTP_SYSTICK_RELOAD_VAL (72000 - 1) // 1ms
#define SNTP_SYSTICK_DURATION 0.001ULLK // 1ms
#define SNTP_SYSTICK_RATE 1000 // 1000 Hz
#define SNTP_SYSTICK_US 1000

extern fix_s31_32_t clock_offset;
extern fix_32_32_t roundtrip_delay;
extern const OSC_Query_Item sntp_tree [5];

void sntp_reset(void);
uint32_t sntp_uptime(void);
void sntp_timestamp_refresh(int64_t tick, OSC_Timetag *now, OSC_Timetag *offset);
uint16_t sntp_request(uint8_t *buf, OSC_Timetag t3);
void sntp_dispatch(uint8_t *buf, OSC_Timetag t4);

#endif // _SNTP_H_

D ipv4ll/ipv4ll.c => ipv4ll/ipv4ll.c +0 -88
@@ 1,88 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#include <stdlib.h>
#include <string.h>

#include <chimaera.h>
#include <chimutil.h>
#include <ipv4ll.h>
#include <config.h>
#include <arp.h>
#include <mdns-sd.h>

static const uint8_t link_local_gateway [] = {169, 254, 0, 0};
static const uint8_t link_local_subnet [] = {255, 255, 0, 0};

static void 
_IPv4LL_random(uint8_t *ip)
{
	ip[0] = 169;
	ip[1] = 254;
	ip[2] = 1 + rand() /(RAND_MAX / 253);
	ip[3] = rand() /(RAND_MAX / 255);
}

void
IPv4LL_claim(uint8_t *ip, uint8_t *gateway, uint8_t *subnet)
{
	uint8_t link_local_ip [4];

	do {
		_IPv4LL_random(link_local_ip);
	} while(arp_probe(SOCK_ARP, link_local_ip)); // collision?

	arp_announce(SOCK_ARP, link_local_ip);

	memcpy(ip, link_local_ip, 4);
	memcpy(gateway, link_local_gateway, 4);
	memcpy(subnet, link_local_subnet, 4);

	//FIXME actually, the user should do this before enabling IPv4LL, not?
	uint8_t brd [4];
	broadcast_address(brd, ip, subnet);
	memcpy(config.output.osc.socket.ip, brd, 4);
	//memcpy(config.config.osc.socket.ip, brd, 4); //FIXME remove
	memcpy(config.sntp.socket.ip, brd, 4);
	//memcpy(config.debug.osc.socket.ip, brd, 4); //FIXME remove
}

/*
 * Config
 */

static uint_fast8_t
_ipv4ll_enabled(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	uint8_t enabled = config.ipv4ll.enabled;

	// needs a config save and reboot to take action
	uint_fast8_t ret = config_check_bool(path, fmt, argc, buf, &config.ipv4ll.enabled);

	if(config.mdns.socket.enabled && (enabled != config.ipv4ll.enabled) )
		mdns_update(); // announce new name

	return ret;
}

/*
 * Query
 */

const OSC_Query_Item ipv4ll_tree [] = {
	OSC_QUERY_ITEM_METHOD("enabled", "Enable/disable", _ipv4ll_enabled, config_boolean_args),
};

D mdns-sd/mdns-sd.c => mdns-sd/mdns-sd.c +0 -840
@@ 1,840 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#include "mdns-sd_private.h"

#include <chimaera.h>
#include <chimutil.h>
#include <wiz.h>
#include <config.h>
#include <sntp.h>

#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#include <libmaple/systick.h>

static const char *localdomain = "local";
static const char *in_addr = "in-addr";
static const char *arpa = "arpa";

// running mDNS resolve request
static DNS_Resolve resolve;

// dns-sd PTR methods prototype definitions
static void _hook_services(DNS_Query *);
static void _hook_osc(DNS_Query *);
static void _hook_instance(DNS_Query *);
static void _hook_arpa(DNS_Query *);

enum {HOOK_SERVICES=0, HOOK_OSC=1, HOOK_INSTANCE=2, HOOK_ARPA=3};

// self query name for DNS lookup
static int len_self;
static char hook_self [NAME_LENGTH+8];

// self query name for DNS revers-lookup
static int len_arpa;
static char hook_arpa [32];

// self instance name for DNS-SD
static int len_instance;
static char hook_instance_udp [NAME_LENGTH+20];
static char hook_instance_tcp [NAME_LENGTH+20];

// dns-sd PTR methods array
static DNS_PTR_Method hooks_udp [] = {
	[HOOK_SERVICES]	= {"\011_services\007_dns-sd\004_udp\005local", _hook_services},
	[HOOK_OSC]			= {"\004_osc\004_udp\005local", _hook_osc},
	[HOOK_INSTANCE]	= {hook_instance_udp, _hook_instance},
	[HOOK_ARPA]			= {hook_arpa, _hook_arpa},
	{NULL, NULL}
};

static DNS_PTR_Method hooks_tcp [] = {
	[HOOK_SERVICES]	= {"\011_services\007_dns-sd\004_udp\005local", _hook_services},
	[HOOK_OSC]			= {"\004_osc\004_tcp\005local", _hook_osc},
	[HOOK_INSTANCE]	= {hook_instance_tcp, _hook_instance},
	[HOOK_ARPA]			= {hook_arpa, _hook_arpa},
	{NULL, NULL}
};

// unrolled query name label
static char qname_unrolled [32]; //TODO big enough?

// unroll query name
static uint8_t *
_unroll_qname(DNS_Query *query, uint8_t *buf, char **qname)
{
	uint8_t *buf_ptr;
	char *ref = qname_unrolled;

	*qname = ref;

	for(buf_ptr=buf; *buf_ptr; )
		if( (buf_ptr[0] & 0xc0) == 0xc0) // it's a pointer at the beginning/end of the label
		{
			uint16_t offset =((buf_ptr[0] & 0x3f) << 8) | (buf_ptr[1] & 0xff); // get offset of label
			char *ptr = (char *)query + offset; // label
			memcpy(ref, ptr, strlen(ptr) + 1); // trailing zero
			buf_ptr += 2; // skip pointer

			return buf_ptr; // that's the end of the label
		}
		else // copy character of label
		{
			*ref = *buf_ptr;
			ref++;
			buf_ptr++;
		}

	*ref = 0x0; // write trailing zero
	buf_ptr++; // skip trailing zero

	return buf_ptr;
}

static uint8_t *
_serialize_query(uint8_t *buf, uint16_t id, uint16_t flags, uint16_t qdcount, uint16_t ancount, uint16_t nscount, uint16_t arcount)
{
	uint8_t *buf_ptr = buf;

	DNS_Query q;
	q.ID = hton(id);
	q.FLAGS = hton(flags);
	q.QDCOUNT = hton(qdcount); // number of questions
	q.ANCOUNT = hton(ancount); // number of answers
	q.NSCOUNT = hton(nscount); // number of authoritative records
	q.ARCOUNT = hton(arcount); // number of resource records

	memcpy(buf_ptr, &q, sizeof(DNS_Query));
	buf_ptr += sizeof(DNS_Query);

	return buf_ptr;
}

static uint8_t *
_serialize_answer_inline(uint8_t *buf, char *qname, uint16_t rtype, uint16_t rclass, uint32_t ttl, uint16_t **rlen)
{
	uint8_t *buf_ptr = buf;

	uint16_t len = strlen(qname) + 1;
	memcpy(buf_ptr, qname, len);
	buf_ptr += len;

	DNS_Answer a;
	a.RTYPE = hton(rtype);
	a.RCLASS = hton(rclass);
	a.TTL = htonl(ttl);
	a.RLEN = 0;
	
	*rlen = (uint16_t *)(buf_ptr + offsetof(DNS_Answer, RLEN));

	memcpy(buf_ptr, &a, sizeof(DNS_Answer));
	buf_ptr += sizeof(DNS_Answer);

	return buf_ptr;
}

static uint8_t *
_serialize_answer(uint8_t *buf, char *qname, uint16_t rtype, uint16_t rclass, uint32_t ttl, uint16_t rlen)
{
	uint16_t *len = NULL;
	uint8_t *buf_ptr = _serialize_answer_inline(buf, qname, rtype, rclass, ttl, &len);
	*len = hton(rlen);

	return buf_ptr;
}

static uint8_t *
_serialize_question(uint8_t *buf, char *qname, uint16_t qtype, uint16_t qclass)
{
	uint8_t *buf_ptr = buf;

	uint16_t len = strlen(qname) + 1;
	memcpy(buf_ptr, qname, len);
	buf_ptr += len;

	DNS_Question quest;
	quest.QTYPE = hton(qtype);
	quest.QCLASS = hton(qclass);

	memcpy(buf_ptr, &quest, sizeof(DNS_Question));
	buf_ptr += sizeof(DNS_Question);

	return buf_ptr;
}

static uint8_t *
_serialize_PTR_services(uint8_t *buf, uint16_t rclass, uint32_t ttl)
{
	char *qname;
	uint16_t len;
	uint8_t *buf_ptr = buf;

	DNS_PTR_Method *hooks = NULL;
	if(config.config.osc.mode == OSC_MODE_UDP)
		hooks = hooks_udp;
	else
		hooks = hooks_tcp;

	qname = hooks[HOOK_OSC].name;
	len = strlen(qname) + 1;

	buf_ptr = _serialize_answer(buf_ptr, hooks[HOOK_SERVICES].name, MDNS_TYPE_PTR, rclass, ttl, len);

	memcpy(buf_ptr, qname, len);
	buf_ptr += len;

	return buf_ptr;
}

static uint8_t * 
_serialize_PTR_osc(uint8_t *buf, uint16_t rclass, uint32_t ttl)
{
	char *qname;
	uint16_t len;
	uint8_t *buf_ptr = buf;

	DNS_PTR_Method *hooks = NULL;
	if(config.config.osc.mode == OSC_MODE_UDP)
		hooks = hooks_udp;
	else
		hooks = hooks_tcp;

	qname = hooks[HOOK_INSTANCE].name;
	len = strlen(qname) + 1;

	buf_ptr = _serialize_answer(buf_ptr, hooks[HOOK_OSC].name, MDNS_TYPE_PTR, rclass, ttl, len);

	memcpy(buf_ptr, qname, len);
	buf_ptr += len;

	return buf_ptr;
}

const char *TXT_TXTVERSION = "txtvers=1";
const char *TXT_VERSION = "version=1.1";
const char *TXT_URI = "uri=http://open-music-kontrollers.ch/chimaera";
const char *TXT_TYPES = "type=ifsbhdtTFNISmc";
const char *TXT_FRAMING = "framing=slip";

const char *TXT_RESET [] = {
	[RESET_MODE_FLASH_SOFT] = "reset=soft",
	[RESET_MODE_FLASH_HARD] = "reset=hard",
};
const char *TXT_STATIC = "static=1";
const char *TXT_DHCP = "dhcp=1";
const char *TXT_IPV4LL = "ipv4ll=1";

static uint8_t *
_serialize_TXT_record(uint8_t *buf, const char *record)
{
	uint8_t *buf_ptr = buf;

	uint8_t len = strlen(record);

	if(len > 0)
	{
		*buf_ptr++ = len;
		memcpy(buf_ptr, record, len);
		buf_ptr += len;
	}

	return buf_ptr;
}

static uint8_t *
_serialize_TXT_instance(uint8_t *buf, uint16_t rclass, uint32_t ttl)
{
	uint8_t *buf_ptr = buf;
	uint16_t *len = NULL;

	DNS_PTR_Method *hooks = NULL;
	if(config.config.osc.mode == OSC_MODE_UDP)
		hooks = hooks_udp;
	else
		hooks = hooks_tcp;

	buf_ptr = _serialize_answer_inline(buf_ptr, hooks[HOOK_INSTANCE].name, MDNS_TYPE_TXT, rclass, ttl, &len);

	uint8_t *txt = buf_ptr;
	buf_ptr = _serialize_TXT_record(buf_ptr, TXT_TXTVERSION);
	buf_ptr = _serialize_TXT_record(buf_ptr, TXT_VERSION);
	buf_ptr = _serialize_TXT_record(buf_ptr, TXT_URI);
	buf_ptr = _serialize_TXT_record(buf_ptr, TXT_TYPES);
	if(config.config.osc.mode == OSC_MODE_SLIP)
		buf_ptr = _serialize_TXT_record(buf_ptr, TXT_FRAMING);
	buf_ptr = _serialize_TXT_record(buf_ptr, TXT_RESET[reset_mode]);
	if(config.dhcpc.enabled || config.ipv4ll.enabled)
	{
		if(config.dhcpc.enabled)
			buf_ptr = _serialize_TXT_record(buf_ptr, TXT_DHCP);
		if(config.ipv4ll.enabled)
			buf_ptr = _serialize_TXT_record(buf_ptr, TXT_IPV4LL);
	}
	else
		buf_ptr = _serialize_TXT_record(buf_ptr, TXT_STATIC);
	//TODO send UID?

	*len = hton(buf_ptr - txt);

	return buf_ptr;
}

static uint8_t *
_serialize_SRV_instance(uint8_t *buf, uint16_t rclass, uint32_t ttl)
{
	uint8_t *buf_ptr = buf;

	DNS_PTR_Method *hooks = NULL;
	if(config.config.osc.mode == OSC_MODE_UDP)
		hooks = hooks_udp;
	else
		hooks = hooks_tcp;

	buf_ptr = _serialize_answer(buf_ptr, hooks[HOOK_INSTANCE].name, MDNS_TYPE_SRV, rclass, ttl, 6 + len_self);

	*buf_ptr++ = 0x0; // priority MSB
	*buf_ptr++ = 0x0; // priority LSB

	*buf_ptr++ = 0x0; // weight MSB
	*buf_ptr++ = 0x0; // weight LSB

	uint16_t port = config.config.osc.socket.port[SRC_PORT];
	*buf_ptr++ = port >> 8; // port MSB
	*buf_ptr++ = port & 0xff; // port LSB

	memcpy(buf_ptr, hook_self, len_self);
	buf_ptr += len_self;

	return buf_ptr;
}

static uint8_t *
_serialize_A_instance(uint8_t *buf, uint16_t rclass, uint32_t ttl)
{
	uint8_t *buf_ptr = buf;

	buf_ptr = _serialize_answer(buf_ptr, hook_self, MDNS_TYPE_A, rclass, ttl, 4);

	memcpy(buf_ptr, config.comm.ip, 4);
	buf_ptr += 4;

	return buf_ptr;
}

static uint8_t *
_serialize_PTR_arpa(uint8_t *buf, uint16_t rclass, uint32_t ttl)
{
	uint8_t *buf_ptr = buf;

	buf_ptr = _serialize_answer(buf_ptr, hook_arpa, MDNS_TYPE_PTR, rclass, ttl, len_self);

	memcpy(buf_ptr, hook_self, len_self);
	buf_ptr += len_self;

	return buf_ptr;
}

static void
_hook_services(DNS_Query *query)
{
	uint8_t *head = BUF_O_OFFSET(buf_o_ptr);
	uint8_t *tail = head;
	uint16_t rclass = MDNS_CLASS_FLUSH | MDNS_CLASS_INET;
	uint32_t ttl = MDNS_TTL_75MIN;

	// mandatory
	tail = _serialize_query(tail, query->ID, MDNS_FLAGS_QR | MDNS_FLAGS_AA, 0, 5, 0, 0);
	// recommended by rfc6763
	tail = _serialize_PTR_services(tail, rclass, ttl);
	tail = _serialize_PTR_osc(tail, rclass, ttl);
	tail = _serialize_TXT_instance(tail, rclass, ttl);
	tail = _serialize_SRV_instance(tail, rclass, ttl);
	tail = _serialize_A_instance(tail, rclass, ttl);

	udp_send(config.mdns.socket.sock, BUF_O_BASE(buf_o_ptr), tail-head);
}

static void
_hook_osc(DNS_Query *query)
{
	uint8_t *head = BUF_O_OFFSET(buf_o_ptr);
	uint8_t *tail = head;
	uint16_t rclass = MDNS_CLASS_FLUSH | MDNS_CLASS_INET;
	uint32_t ttl = MDNS_TTL_75MIN;

	// mandatory
	tail = _serialize_query(tail, query->ID, MDNS_FLAGS_QR | MDNS_FLAGS_AA, 0, 4, 0, 0);
	// recommended by rfc6763
	tail = _serialize_PTR_osc(tail, rclass, ttl);
	tail = _serialize_TXT_instance(tail, rclass, ttl);
	tail = _serialize_SRV_instance(tail, rclass, ttl);
	tail = _serialize_A_instance(tail, rclass, ttl);

	udp_send(config.mdns.socket.sock, BUF_O_BASE(buf_o_ptr), tail-head);
}

static void
_hook_instance(DNS_Query *query)
{
	uint8_t *head = BUF_O_OFFSET(buf_o_ptr);
	uint8_t *tail = head;
	uint16_t rclass = MDNS_CLASS_FLUSH | MDNS_CLASS_INET;
	uint32_t ttl = MDNS_TTL_75MIN;

	// mandatory
	tail = _serialize_query(tail, query->ID, MDNS_FLAGS_QR | MDNS_FLAGS_AA, 0, 3, 0, 0);
	// recommended by rfc6763
	tail = _serialize_TXT_instance(tail, rclass, ttl);
	tail = _serialize_SRV_instance(tail, rclass, ttl);
	tail = _serialize_A_instance(tail, rclass, ttl);

	udp_send(config.mdns.socket.sock, BUF_O_BASE(buf_o_ptr), tail-head);
}

static void
_hook_arpa(DNS_Query *query)
{
	uint8_t *head = BUF_O_OFFSET(buf_o_ptr);
	uint8_t *tail = head;
	uint16_t rclass = MDNS_CLASS_FLUSH | MDNS_CLASS_INET;
	uint32_t ttl = MDNS_TTL_75MIN;

	tail = _serialize_query(tail, query->ID, MDNS_FLAGS_QR | MDNS_FLAGS_AA, 0, 1, 0, 0);
	tail = _serialize_PTR_arpa(tail, rclass, ttl);

	udp_send(config.mdns.socket.sock, BUF_O_BASE(buf_o_ptr), tail-head);
}

static uint16_t
_update_hook_self(char *name)
{
	//TODO check maximal length

	int len;
	char *ptr = hook_self;

	len = strlen(name);
	*ptr++ = len;
	memcpy(ptr, name, len);
	ptr += len;

	len = strlen(localdomain);
	*ptr++ = len;
	memcpy(ptr, localdomain, len);
	ptr += len;

	*ptr = 0x0; // trailing zero
	
	return strlen(hook_self) + 1;
}

static uint16_t
_update_hook_instance(char *name)
{
	int len;

	// UDP
	char *ptr = hook_instance_udp;
	const char *instance_postfix = hooks_udp[HOOK_OSC].name;

	len = strlen(name);
	*ptr++ = len;
	memcpy(ptr, name, len);
	ptr += len;

	len = strlen(instance_postfix);
	memcpy(ptr, instance_postfix, len);
	ptr += len;

	*ptr = 0x0; // trailing zero

	// TCP
	ptr = hook_instance_tcp;
	instance_postfix = hooks_tcp[HOOK_OSC].name;

	len = strlen(name);
	*ptr++ = len;
	memcpy(ptr, name, len);
	ptr += len;

	len = strlen(instance_postfix);
	memcpy(ptr, instance_postfix, len);
	ptr += len;

	*ptr = 0x0; // trailing zero
	
	return strlen(hook_instance_tcp) + 1;
}

static uint16_t
_update_hook_arpa(uint8_t *ip)
{
	int len;
	int i;
	char octet [4];
	char *ptr = hook_arpa;

	// octets 4-1
	for(i=3; i>=0; i--)
	{
		sprintf(octet, "%d", ip[i]);
		len = strlen(octet);
		*ptr++ = len;
		memcpy(ptr, octet, len);
		ptr += len;
	}

	// in-addr
	len = strlen(in_addr);
	*ptr++ = len;
	memcpy(ptr, in_addr, len);
	ptr += len;

	// arpa
	len = strlen(arpa);
	*ptr++ = len;
	memcpy(ptr, arpa, len);
	ptr += len;

	*ptr = 0x0; // trailing zero

	return strlen(hook_arpa) + 1;
}

static uint8_t *
_dns_question(DNS_Query *query, uint8_t *buf)
{
	uint8_t *buf_ptr = buf;

	DNS_PTR_Method *hooks = NULL;
	if(config.config.osc.mode == OSC_MODE_UDP)
		hooks = hooks_udp;
	else
		hooks = hooks_tcp;

	char *qname;
	buf_ptr = _unroll_qname(query, buf_ptr, &qname);

	DNS_Question *question =(DNS_Question *)buf_ptr;
	question->QTYPE = hton(question->QTYPE);
	question->QCLASS = hton(question->QCLASS);
	buf_ptr += sizeof(DNS_Question);

	// TODO answer as unicast
	//uint_fast8_t unicast = question->QCLASS & MDNS_CLASS_UNICAST;

	if( (question->QCLASS & 0x7fff) == MDNS_CLASS_INET) // check question class
	{
		// for IP mDNS lookup
		if(  (question->QTYPE == MDNS_TYPE_A)
			|| (question->QTYPE == MDNS_TYPE_AAAA)
			|| (question->QTYPE == MDNS_TYPE_ANY) )
		{
			// reply with current IP when there is a request for our name
			if(!strncmp(hook_self, qname, len_self))
			{
				uint8_t *head = BUF_O_OFFSET(buf_o_ptr);
				uint8_t *tail = head;

				tail = _serialize_query(tail, query->ID, MDNS_FLAGS_QR | MDNS_FLAGS_AA, 0, 1, 0, 0);
				//FIXME append negative response for IPv6 via NSEC record
				tail = _serialize_answer(tail, hook_self, MDNS_TYPE_A, MDNS_CLASS_FLUSH | MDNS_CLASS_INET, MDNS_TTL_75MIN, 4);

				memcpy(tail, config.comm.ip, 4);
				tail += 4;

				udp_send(config.mdns.socket.sock, BUF_O_BASE(buf_o_ptr), tail-head);
			}
		}

		// for mDNS reverse-lookup and for mDNS-SD(service discovery)
		if( (question->QTYPE == MDNS_TYPE_PTR) || (question->QTYPE == MDNS_TYPE_ANY) )
		{
			DNS_PTR_Method *hook;
			for(hook=hooks; hook->name; hook++)
				if(!strcmp(hook->name, qname)) // is there a hook with this qname registered?
					hook->cb(query);
		}
	}

	return buf_ptr;
}

static uint8_t *
_dns_answer(DNS_Query *query, uint8_t *buf)
{
	uint8_t *buf_ptr = buf;

	char *qname;
	buf_ptr = _unroll_qname(query, buf_ptr, &qname);

	DNS_Answer *answer =(DNS_Answer *)buf_ptr;
	answer->RTYPE = hton(answer->RTYPE);
	answer->RCLASS = hton(answer->RCLASS);
	answer->TTL = htonl(answer->TTL);
	answer->RLEN = hton(answer->RLEN);
	buf_ptr += sizeof(DNS_Answer);

	switch(answer->RTYPE)
	{
		case MDNS_TYPE_A:
			// ignore when qname was not requested by us previously
			if(!resolve.cb || strcmp(qname, resolve.name))
				break;
		
			//TODO handle conflicts when probing

			uint8_t *ip = (uint8_t *)buf_ptr; 
			if(ip_part_of_subnet(ip)) // check IP for same subnet TODO make this configurable
			{
				timer_pause(mdns_timer); // stop timeout timer

				resolve.cb(ip, resolve.data);

				// reset request
				resolve.name[0] = '\0';
				resolve.cb = NULL;
				resolve.data = NULL;
			}
			break;
		default:
			// ignore remaining answer types, e.g MDNS_TYPE_PTR, MDNS_TYPE_SRV, MDNS_TYPE_TXT
			break;
	}

	buf_ptr += answer->RLEN; // skip rdata

	return buf_ptr;
}

void 
mdns_dispatch(uint8_t *buf, uint16_t len)
{
	(void)len;
	// update qname labels corresponding to self
	len_self = _update_hook_self(config.name);
	len_instance = _update_hook_instance(config.name);
	len_arpa = _update_hook_arpa(config.comm.ip);

	uint8_t *buf_ptr = buf;
	DNS_Query *query =(DNS_Query *)buf_ptr;

	// convert from network byteorder
	query->ID = hton(query->ID);
	query->FLAGS = hton(query->FLAGS);
	query->QDCOUNT = hton(query->QDCOUNT);
	query->ANCOUNT = hton(query->ANCOUNT);
	query->NSCOUNT = hton(query->NSCOUNT);
	query->ARCOUNT = hton(query->ARCOUNT);

	uint8_t qr =(query->FLAGS & MDNS_FLAGS_QR) >> MDNS_FLAGS_QR_BIT;
	//uint8_t opcode =(query->FLAGS & MDNS_FLAGS_OPCODE) >> MDNS_FLAGS_OPCODE_SHIFT;
	//uint8_t aa =(query->FLAGS & MDNS_FLAGS_AA) >> MDNS_FLAGS_AA_BIT;
	//uint8_t tc =(query->FLAGS & MDNS_FLAGS_TC) >> MDNS_FLAGS_TC_BIT;
	//uint8_t rd =(query->FLAGS & MDNS_FLAGS_RD) >> MDNS_FLAGS_RD_BIT;
	//uint8_t ra =(query->FLAGS & MDNS_FLAGS_RA) >> MDNS_FLAGS_RA_BIT;
	//uint8_t z =(query->FLAGS & MDNS_FLAGS_Z) >> MDNS_FLAGS_Z_SHIFT;
	//uint8_t rcode =(query->FLAGS & MDNS_FLAGS_RCODE) >> MDNS_FLAGS_RCODE_SHIFT;

	buf_ptr += sizeof(DNS_Query);

	uint_fast8_t i;
	switch(qr)
	{
		case 0x0: // mDNS Query Question
			for(i=0; i<query->QDCOUNT; i++) // walk all questions
				buf_ptr = _dns_question(query, buf_ptr);

			// ignore answers
			break;
		case 0x1: // mDNS Query Answer
			for(i=0; i<query->QDCOUNT; i++) // skip all questions
				buf_ptr += strlen((char *)buf_ptr) + 1 + sizeof(DNS_Question);

			for(i=0; i<query->ANCOUNT; i++) // walk all answers
				buf_ptr = _dns_answer(query, buf_ptr);
			break;
	}
}

#if 0
void mdns_probe()
{
	len_self = _update_hook_self(config.name);
	len_instance = _update_hook_instance(config.name);
	len_arpa = _update_hook_arpa(config.comm.ip);

	int i;
	for(i=0; i<3; i++)
	{
		uint8_t *head = BUF_O_OFFSET(buf_o_ptr);
		uint8_t *tail = head;
		
		uint16_t id = rand() & 0xffff;
		tail = _serialize_query(tail, id, MDNS_FLAGS_AA, 0, 1, 0, 0);
		tail = _serialize_question(tail, hook_self, MDNS_TYPE_ANY, MDNS_CLASS_INET);
		
		udp_send(config.mdns.socket.sock, BUF_O_BASE(buf_o_ptr), tail-head);

		// wait 250ms second
		uint32_t tick = systick_uptime();
		while(systick_uptime() - tick < SNTP_SYSTICK_RATE / 4)
			;

		//FIXME listen for collisions with hook_self
	}
}
#endif

static void
_mdns_tell(int count, uint16_t rclass, uint32_t ttl)
{
	len_self = _update_hook_self(config.name);
	len_instance = _update_hook_instance(config.name);
	len_arpa = _update_hook_arpa(config.comm.ip);

	int i;
	for(i=0; i<count; i++)
	{
		uint8_t *head = BUF_O_OFFSET(buf_o_ptr);
		uint8_t *tail = head;
		
		uint16_t id = rand() & 0xffff;
		tail = _serialize_query(tail, id, MDNS_FLAGS_QR | MDNS_FLAGS_AA, 0, 5, 0, 0);
		tail = _serialize_answer(tail, hook_self, MDNS_TYPE_A, rclass, ttl, 4);
		
		memcpy(tail, config.comm.ip, 4);
		tail += 4;

		// append dns-sd services here, too
		tail = _serialize_PTR_services(tail, rclass, ttl);
		tail = _serialize_PTR_osc(tail, rclass, ttl);
		tail = _serialize_TXT_instance(tail, rclass, ttl);
		tail = _serialize_SRV_instance(tail, rclass, ttl);
		tail = _serialize_A_instance(tail, rclass, ttl);
		
		udp_send(config.mdns.socket.sock, BUF_O_BASE(buf_o_ptr), tail-head);

		if(i == count-1)
			break;

		// wait a second
		uint32_t tick = systick_uptime();
		while(systick_uptime() - tick < SNTP_SYSTICK_RATE)
			;
	}
}

void
mdns_announce()
{
	_mdns_tell(2, MDNS_CLASS_FLUSH | MDNS_CLASS_INET, MDNS_TTL_75MIN);
}

void
mdns_update()
{
	_mdns_tell(1, MDNS_CLASS_FLUSH | MDNS_CLASS_INET, MDNS_TTL_75MIN);
}

void
mdns_goodbye()
{
	_mdns_tell(1, MDNS_CLASS_FLUSH | MDNS_CLASS_INET, MDNS_TTL_NULL);
}

void
mdns_resolve_timeout()
{
	if(resolve.cb)
		resolve.cb(NULL, resolve.data);

	resolve.name[0] = '\0';
	resolve.cb = NULL;
	resolve.data = NULL;
}

//TODO implement timeout
uint_fast8_t
mdns_resolve(const char *name, mDNS_Resolve_Cb cb, void *data)
{
	if(resolve.cb) // is there a mDNS resolve request already ongoing?
		return 0;

	// construct DNS label from mDNS name
	char *ref = resolve.name;
	const char *s0 = name;
	char *s1;
	char len;
	while( (s1 = strchr(s0, '.')) )
	{
		len = s1 - s0;
		*ref++ = len;
		memcpy(ref, s0, len);
		ref += len;

		s0 = s1 + 1; // skip '.'
	}
	len = strlen(s0);
	*ref++ = len;
	memcpy(ref, s0, len);
	ref += len;
	*ref++ = 0x0; // trailing zero

	resolve.cb = cb;
	resolve.data = data;

	// serialize and send mDNS name request
	uint8_t *head = BUF_O_OFFSET(buf_o_ptr);
	uint8_t *tail = head;

	uint16_t id = rand() & 0xffff;
	tail = _serialize_query(tail, id, 0, 1, 0, 0, 0);
	tail = _serialize_question(tail, resolve.name, MDNS_TYPE_A, MDNS_CLASS_INET);
	
	udp_send(config.mdns.socket.sock, BUF_O_BASE(buf_o_ptr), tail-head);

	// start timer for timeout
	timer_pause(mdns_timer);
	mdns_timer_reconfigure();
	timer_resume(mdns_timer);

	return 1;
}

/*
 * Config
 */

static uint_fast8_t
_mdns_enabled(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	// needs a config save and reboot to take action
	return config_socket_enabled(&config.mdns.socket, path, fmt, argc, buf);
}

/*
 * Query
 */

const OSC_Query_Item mdns_tree [] = {
	OSC_QUERY_ITEM_METHOD("enabled", "Enable/disable", _mdns_enabled, config_boolean_args),
};

D mdns-sd/mdns-sd_private.h => mdns-sd/mdns-sd_private.h +0 -105
@@ 1,105 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _MDNS_PRIVATE_H
#define _MDNS_PRIVATE_H

#include <mdns-sd.h>

typedef struct _DNS_Query DNS_Query;
typedef struct _DNS_Question DNS_Question;
typedef struct _DNS_Answer DNS_Answer;
typedef struct _DNS_Resolve DNS_Resolve;
typedef struct _DNS_PTR_Method DNS_PTR_Method;
typedef void(*DNS_PTR_Method_Cb)(DNS_Query *query);

#define MDNS_FLAGS_QR_BIT				15
#define MDNS_FLAGS_OPCODE_SHIFT	11
#define MDNS_FLAGS_AA_BIT				10
#define MDNS_FLAGS_TC_BIT				9
#define MDNS_FLAGS_RD_BIT				8
#define MDNS_FLAGS_RA_BIT				7
#define MDNS_FLAGS_Z_SHIFT			4
#define MDNS_FLAGS_RCODE_SHIFT	0

#define MDNS_FLAGS_QR						(0x1 << MDNS_FLAGS_QR_BIT)
#define MDNS_FLAGS_OPCODE 			(0xf << MDNS_FLAGS_OPCODE_SHIFT)
#define MDNS_FLAGS_AA						(0x1 << MDNS_FLAGS_AA_BIT)
#define MDNS_FLAGS_TC						(0x1 << MDNS_FLAGS_TC_BIT)
#define MDNS_FLAGS_RD						(0x1 << MDNS_FLAGS_RD_BIT)
#define MDNS_FLAGS_RA						(0x1 << MDNS_FLAGS_RA_BIT)
#define MDNS_FLAGS_Z						(0x7 << MDNS_FLAGS_Z_SHIFT)
#define MDNS_FLAGS_RCODE				(0xf << MDNS_FLAGS_RCODE_SHIFT)

struct _DNS_PTR_Method {
	char *name;
	DNS_PTR_Method_Cb cb;
};

struct _DNS_Query {
	uint16_t ID;
	uint16_t FLAGS;
	/*
	uint8_t QR : 1; // 0: query, 1: response
	uint8_t Opcode : 4; // 0: standard query
	uint8_t AA : 1; // authoritative answer?
	uint8_t TC : 1; // truncated message?
	uint8_t RD : 1; // recursion desired
	uint8_t RA : 1; // recursion available
	uint8_t Z : 3; // reserved
	uint8_t RCODE : 4; // 0: no error, 1: format error, 2: server failure, 3: name error, 4: not implemented, 5: refused
	*/
	uint16_t QDCOUNT; // number of entries in question section
	uint16_t ANCOUNT; // number of entries in answer section
	uint16_t NSCOUNT; // number of name server resource records in autoritative records section
	uint16_t ARCOUNT; // number of resource records in additional records section
} __attribute((packed,aligned(2)));

#define MDNS_TYPE_A						0x0001
#define MDNS_TYPE_AAAA				0x001c
#define MDNS_TYPE_PTR					0x000c
#define MDNS_TYPE_TXT					0x0010
#define MDNS_TYPE_SRV					0x0021
#define MDNS_TYPE_ANY					0x00ff

#define MDNS_CLASS_INET				0x0001
#define MDNS_CLASS_FLUSH			0x8000
#define MDNS_CLASS_UNICAST		0x8000

#define MDNS_TTL_75MIN			0x00001194
#define MDNS_TTL_120SEC			0x00000078
#define MDNS_TTL_NULL				0x00000000

struct _DNS_Question {
	uint16_t QTYPE;
	uint16_t QCLASS; // 1: internet
} __attribute((packed,aligned(2)));

struct _DNS_Answer {
	uint16_t RTYPE;
	uint16_t RCLASS;
	uint32_t TTL;
	uint16_t RLEN;
} __attribute((packed,aligned(2)));

struct _DNS_Resolve {
	char name [32];
	mDNS_Resolve_Cb cb;
	void *data;
};

#endif

D ptp/ptp.c => ptp/ptp.c +0 -594
@@ 1,594 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#include <string.h>

#include <libmaple/systick.h>

#include "ptp_private.h"

#include <chimaera.h>
#include <chimutil.h>
#include <config.h>
#include <debug.h>
#include <sntp.h>

/*
 * implementation is based on:
 * Design considerations for software only implementations of the IEEE 1588 precision time protocol
 * K Correll, N Barendt, M Branicky - Conference on IEEE, 2005 - dora.cwru.edu
 */

static int64_t t0 = 0ULL; // us since epoch

static int64_t t1, t2, t3, t4;
static double c1, c2, c3;

#if 0
static int64_t ta, tb, tc, td;
static double ca, cb, cc;
#endif

static double D0, D1, DD0, DD1;
static double O0, O1, OO0, OO1;
static double Os;
static double Ds;

static uint_fast8_t ptp_timescale = 0;
static uint16_t utc_offset;
static uint64_t master_clock_id;
static uint16_t sync_seq_id;
//static uint16_t resp_seq_id;
static uint16_t req_seq_id = 0;
//static uint16_t pdelay_req_seq_id = 0;
static uint_fast8_t sync_counter = 0;

void
ptp_reset()
{
	t0 = 0ULL;

	D0 = D1 = DD0 = DD1 = 0.f;
	O0 = O1 = OO0 = OO1 = 0.f;

	// initialize stiffness
	Ds = 1.f / (double)config.ptp.delay_stiffness;
	Os = 1.f / (double)config.ptp.offset_stiffness;
}

static inline __always_inline void
ptp_request_ntoh(PTP_Request *req)
{
	req->message_length = ntoh(req->message_length);
	req->flags = ntoh(req->flags);
	req->correction = ntohll(req->correction);
	req->clock_identity = ntohll(req->clock_identity);
	req->source_port_id = hton(req->source_port_id);
	req->sequence_id = hton(req->sequence_id);
	req->timestamp.epoch = hton(req->timestamp.epoch);
	req->timestamp.sec = htonl(req->timestamp.sec);
	req->timestamp.nsec = htonl(req->timestamp.nsec);
}

static inline __always_inline void
ptp_response_ntoh(PTP_Response *resp)
{
	resp->requesting_clock_identity = ntohll(resp->requesting_clock_identity);
	resp->requesting_source_port_id = ntoh(resp->requesting_source_port_id);
}

static inline __always_inline void
ptp_announce_ntoh(PTP_Announce *ann)
{
	ann->origin_current_utc_offset = ntoh(ann->origin_current_utc_offset);
	ann->grandmaster_clock_variance = ntoh(ann->grandmaster_clock_variance);
	ann->grandmaster_clock_identity = ntohll(ann->grandmaster_clock_identity);
	ann->local_steps_removed = ntoh(ann->local_steps_removed);
}

#define JAN_1970 2208988800ULLK
#define PTP_TO_US(ts) ((int64_t)(ts).sec * 1000000 + (ts).nsec / 1000)
#define TICK_TO_US(tick) (t0 + (tick))
#define SLAVE_CLOCK_ID (*(uint64_t *)UID_BASE)

// returns microseconds since startup
int64_t __CCM_TEXT__
ptp_uptime()
{
	volatile uint32_t ticks;
	volatile uint32_t cycle_cnt;

	do {
		ticks = systick_uptime();
		cycle_cnt = systick_get_count();
	} while (ticks != systick_uptime());

	int64_t uptime;
	uptime = (int64_t)ticks * SNTP_SYSTICK_US;
	uptime += (SNTP_SYSTICK_RELOAD_VAL + 1 - cycle_cnt) / CYCLES_PER_MICROSECOND;

	return uptime;
}

static void
_ptp_update_offset(void)
{
	// offset = t2 - t1 - delay - correction1 - correction2

	if(t0 == 0ULL)
		t0 = t1 + DD1 - t2;
	else
	{
		int64_t a = t2 - t1;
		double A = a;
		O1 = A - DD1;
		O1 -= c1 + c2;
		OO1 = Os * (O0 + O1) / 2 + OO0 * (1.f - Os);
		t0 -= OO1;

#if 1
	DEBUG("sdd", "PTPv2", OO1, DD1);
#endif

		O0 = O1;
		OO0 = OO1;
	}

#define OFFSET_THRESH 1000.0 // 1s
	if(fabs(OO0) > OFFSET_THRESH)
	{
		timer_pause(ptp_timer);
		ptp_reset();
	}
#undef OFFSET_THRESH

	// schedule delay request
	if(++sync_counter >= config.ptp.multiplier)
	{
		float sec = (float)rand() / RAND_MAX;
		ptp_timer_reconfigure(sec);
		timer_resume(ptp_timer);
		sync_counter = 0;
	}
}

void
ptp_request()
{
	uint8_t *buf = BUF_O_OFFSET(buf_o_ptr);
	static PTP_Request req;

	req.message_id = PTP_MESSAGE_ID_DELAY_REQ;
	req.version = PTP_VERSION_2;
	req.message_length = sizeof(PTP_Request);
	req.flags = 0U;
	req.correction = 0ULL;
	req.clock_identity = SLAVE_CLOCK_ID;
	req.source_port_id = 1U;
	req.sequence_id = ++req_seq_id;
	req.control = PTP_CONTROL_DELAY_REQ;
	req.log_message_interval = 0x7f;
	req.timestamp.epoch = 0U;
	req.timestamp.sec = 0UL;
	req.timestamp.nsec = 0UL;

	ptp_request_ntoh(&req);
	memcpy(buf, &req, sizeof(PTP_Request));
	
	udp_send(config.ptp.event.sock, BUF_O_BASE(buf_o_ptr), sizeof(PTP_Request)); // port 319
	t3 = TICK_TO_US(ptp_uptime());
}

static void
_ptp_update_delay_e2e(void)
{
	int64_t a = t4 - t1;
	int64_t b = t3 - t2;

	double A = a;
	double B = b;

	D1 = A - B;
	D1 -= c1 + c2 + c3;
	D1 /= 2;

	if(D0 == 0.f)
		DD0 = D0 = D1;

	DD1 = Ds * (D0 + D1) / 2 + DD0 * (1.f - Ds);

	D0 = D1;
	DD0 = DD1;

#define DELAY_THRESH 1000.f // 1s
	if(DD0 > DELAY_THRESH)
	{
		timer_pause(ptp_timer);
		ptp_reset();
	}
#undef DELAY_THRESH
}

#if 0 // we need two more sockets for P2P mode
static void
_ptp_update_delay_p2p()
{
	int64_t a = td - ta;
	int64_t b = tc - tb;

	D1 = A - B;
	D1 -= ca + cb + cc;
	D1 /= 2;

	DD1 = Ds * (D0 + D1) / 2 + DD0 * (1.f - Ds);

	D0 = D1;
	DD0 = DD1;
}
#endif

static void
_ptp_dispatch_announce(PTP_Announce *ann)
{
	master_clock_id = ann->req.clock_identity;
	utc_offset = ann->origin_current_utc_offset;
	//TODO
}

static void
_ptp_dispatch_sync(PTP_Sync *sync, int64_t tick)
{
	if(master_clock_id != sync->clock_identity)
		return; // not our master

	t2 = TICK_TO_US(tick); // set receive timestamp
	c2 = sync->correction * 0x0.0001p0;
	
	ptp_timescale = sync->flags & PTP_FLAGS_TIMESCALE; // are we using PTP timescale?

	if(sync->flags & PTP_FLAGS_TWO_STEP)
		sync_seq_id = sync->sequence_id; // wait for follow_up message
	else
	{
		t1 = PTP_TO_US(sync->timestamp);
		c1 = sync->correction * 0x0.0001p0;
		_ptp_update_offset();
	}
}

static void
_ptp_dispatch_follow_up(PTP_Follow_Up *folup)
{
	if(master_clock_id != folup->clock_identity)
		return; // not our master

	if(sync_seq_id != folup->sequence_id)
		return; // does not match previously received sync packet

	t1 = PTP_TO_US(folup->timestamp);
	c1 = folup->correction * 0x0.0001p0;
	_ptp_update_offset();
}

static void
_ptp_dispatch_delay_resp(PTP_Response *resp)
{
	if(master_clock_id != resp->req.clock_identity)
		return; // not our master

	if(req_seq_id != resp->req.sequence_id)
		return; // not response for us

	if(SLAVE_CLOCK_ID != resp->requesting_clock_identity)
		return;

	t4 = PTP_TO_US(resp->req.timestamp);
	c3 = resp->req.correction * 0x0.0001p0;
	_ptp_update_delay_e2e();
}

#if 0 // we need two more sockets for P2P mode
static void
_ptp_dispatch_pdelay_req(PTP_Request *req, int64_t tick)
{
	static PTP_Response resp;
	uint8_t *offset = BUF_O_OFFSET(buf_o_ptr);
	uint8_t *base = BUF_O_BASE(buf_o_ptr);

	if(master_clock_id != req->clock_identity)
		return;

	// PEER DELAY RESPONSE
	int64_t tx = TICK_TO_US(tick);

	resp.req.message_id = PTP_MESSAGE_ID_PDELAY_RESP;
	resp.req.version = PTP_VERSION_2;
	resp.req.message_length = sizeof(PTP_Response);
	resp.req.flags = PTP_FLAGS_TWO_STEP;
	resp.req.correction = 0.f;
	resp.req.clock_identity = SLAVE_CLOCK_ID;
	resp.req.source_port_id = 1U;
	resp.req.sequence_id = req->sequence_id;
	resp.req.control = PTP_CONTROL_OTHER;
	resp.req.log_message_interval = 0x7f;

	resp.req.timestamp.epoch = 0U;
	resp.req.timestamp.sec = tx / 1000000;
	resp.req.timestamp.nsec = (tx - resp.req.timestamp.sec) * 1000000000;

	resp.requesting_clock_identity = req->clock_identity;
	resp.requesting_source_port_id = req->source_port_id;

	memcpy(offset, &resp, sizeof(PTP_Response));
	ptp_request_ntoh((PTP_Request *)offset);
	ptp_response_ntoh((PTP_Response *)offset);
	udp_send(config.ptp.event.sock, base, sizeof(PTP_Response)); // port 319

	// PEER DELAY RESPONSE FOLLOW UP
	int64_t ty = TICK_TO_US(ptp_uptime());

	resp.req.message_id = PTP_MESSAGE_ID_PDELAY_RESP_FOLLOW_UP;
	resp.req.flags = 0U;
	resp.req.timestamp.sec = ty / 1000000;
	resp.req.timestamp.nsec = (ty - resp.req.timestamp.sec) * 1000000000;

	memcpy(offset, &resp, sizeof(PTP_Response));
	ptp_request_ntoh((PTP_Request *)offset);
	ptp_response_ntoh((PTP_Response *)offset);
	udp_send(config.ptp.general.sock, base, sizeof(PTP_Response)); // port 320

	// PEER DELAY REQUEST
	resp.req.message_id = PTP_MESSAGE_ID_PDELAY_REQ;
	resp.req.message_length = sizeof(PTP_Request);
	resp.req.sequence_id = ++pdelay_req_seq_id;
	resp.req.timestamp.sec = 0UL;
	resp.req.timestamp.nsec = 0UL;

	ptp_request_ntoh(&resp.req);

	memcpy(offset, &resp, sizeof(PTP_Request));
	ptp_request_ntoh((PTP_Request *)offset);
	udp_send(config.ptp.event.sock, base, sizeof(PTP_Response)); // port 319

	ta = TICK_TO_US(ptp_uptime());
	ca = req->correction * 0x0.0001p0;
}

static void
_ptp_dispatch_pdelay_resp(PTP_Response *resp, int64_t tick)
{
	if(master_clock_id != resp->req.clock_identity)
		return;

	if(pdelay_req_seq_id != resp->req.sequence_id)
		return;

	if(SLAVE_CLOCK_ID != resp->requesting_clock_identity)
		return;

	tb = PTP_TO_US(resp->req.timestamp);
	cb = resp->req.correction * 0x0.0001p0;
	tc = TICK_TO_US(tick);
	// now wait for follow up
}

static void
_ptp_dispatch_pdelay_resp_follow_up(PTP_Response *resp)
{
	if(master_clock_id != resp->req.clock_identity)
		return;

	if(pdelay_req_seq_id != resp->req.sequence_id)
		return;

	if(SLAVE_CLOCK_ID != resp->requesting_clock_identity)
		return;

	td = PTP_TO_US(resp->req.timestamp);
	cc = resp->req.correction * 0x0.0001p0;
	_ptp_update_delay_p2p();
}
#endif

void
ptp_timestamp_refresh(int64_t tick, OSC_Timetag *now, OSC_Timetag *offset)
{
	uint64_t ts;

	ts = TICK_TO_US(tick);

	*now = ts * 0.000001;
	*now += JAN_1970;
	*now -= ptp_timescale ? utc_offset : 0;

	if(offset)
	{
		if( (config.output.offset > 0ULLK) && (t0 != 0ULL) )
			*offset = *now + config.output.offset;
		else
			*offset = OSC_IMMEDIATE;
	}
}

void
ptp_dispatch(uint8_t *buf, int64_t tick)
{
	PTP_Request *msg = (PTP_Request *)buf;

	// byte swap short message part
	ptp_request_ntoh(msg);

	// check for PTP version 2
	if( (msg->version & 0xf) != PTP_VERSION_2)
		return;

	// byte swap extended message part
	if(msg->message_length == sizeof(PTP_Response))
		ptp_response_ntoh((PTP_Response *)msg);
	else if(msg->message_length == sizeof(PTP_Announce))
		ptp_announce_ntoh((PTP_Announce *)msg);

	PTP_Message_ID msg_id = msg->message_id & 0xf;
	switch(msg_id)
	{
		// event port
		case PTP_MESSAGE_ID_SYNC:
			_ptp_dispatch_sync(msg, tick);
			break;
		case PTP_MESSAGE_ID_DELAY_REQ:
			// we only act as PTP slave, not as master
			break;
		case PTP_MESSAGE_ID_PDELAY_REQ:
			// we need two more sockets for P2P mode
			//_ptp_dispatch_pdelay_req(msg, tick);
			break;

		// general port
		case PTP_MESSAGE_ID_PDELAY_RESP:
			// we need two more sockets for P2P mode
			//_ptp_dispatch_pdelay_resp((PTP_Response *)msg, tick);
			break;
		case PTP_MESSAGE_ID_FOLLOW_UP:
			_ptp_dispatch_follow_up(msg);
			break;
		case PTP_MESSAGE_ID_DELAY_RESP:
			_ptp_dispatch_delay_resp((PTP_Response *)msg);
			break;
		case PTP_MESSAGE_ID_PDELAY_RESP_FOLLOW_UP:
			// we need two more sockets for P2P mode
			//_ptp_dispatch_pdelay_resp_follow_up((PTP_Response *)msg);
			break;
		case PTP_MESSAGE_ID_ANNOUNCE:
			_ptp_dispatch_announce((PTP_Announce *)msg);
			break;
		case PTP_MESSAGE_ID_SIGNALING:
			// ignored for now
			break;
		case PTP_MESSAGE_ID_MANAGEMENT:
			// ignored for now
			break;
	}
}

/*
 * Config
 */

static uint_fast8_t
_ptp_enabled(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	uint_fast8_t ret;

	ptp_reset();
	ret = config_socket_enabled(&config.ptp.event, path, fmt, argc, buf);
	if(ret && config.sntp.socket.enabled && config.ptp.event.enabled) // automatically disable sntp
		sntp_enable(0);

	return ret;
}

static uint_fast8_t
_ptp_multiplier(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	return config_check_uint8(path, fmt, argc, buf, &config.ptp.multiplier);
}

static uint_fast8_t
_ptp_offset_stiffness(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	uint_fast8_t res = config_check_uint8(path, fmt, argc, buf, &config.ptp.offset_stiffness);
	Os = 1.f / (double)config.ptp.offset_stiffness;
	return res;
}

static uint_fast8_t
_ptp_delay_stiffness(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	uint_fast8_t res = config_check_uint8(path, fmt, argc, buf, &config.ptp.delay_stiffness);
	Ds = 1.f / (double)config.ptp.delay_stiffness;
	return res;
}

static uint_fast8_t
_ptp_offset(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	(void)fmt;
	(void)argc;
	osc_data_t *buf_ptr = buf;
	uint16_t size;
	int32_t uuid;

	buf_ptr = osc_get_int32(buf_ptr, &uuid);

	float fOO1 = OO1;
	size = CONFIG_SUCCESS("isf", uuid, path, fOO1);

	CONFIG_SEND(size);

	return 1;
}

static uint_fast8_t
_ptp_delay(const char *path, const char *fmt, uint_fast8_t argc, osc_data_t *buf)
{
	(void)fmt;
	(void)argc;
	osc_data_t *buf_ptr = buf;
	uint16_t size;
	int32_t uuid;

	buf_ptr = osc_get_int32(buf_ptr, &uuid);

	float fDD1 = DD1;
	size = CONFIG_SUCCESS("isf", uuid, path, fDD1);

	CONFIG_SEND(size);

	return 1;
}

/*
 * Query
 */

static const OSC_Query_Argument ptp_multiplier_args [] = {
	OSC_QUERY_ARGUMENT_INT32("Rate", OSC_QUERY_MODE_RW, 1, 16, 1)
};

static const OSC_Query_Argument ptp_stiffness_args [] = {
	OSC_QUERY_ARGUMENT_INT32("Stiffness", OSC_QUERY_MODE_RW, 1, 128, 1)
};

static const OSC_Query_Argument ptp_offset_args [] = {
	OSC_QUERY_ARGUMENT_FLOAT("Microseconds", OSC_QUERY_MODE_R, -100.f, 100.f, 0.f)
};

static const OSC_Query_Argument ptp_delay_args [] = {
	OSC_QUERY_ARGUMENT_FLOAT("Microseconds", OSC_QUERY_MODE_R, 0.f, 100.f, 0.f)
};

const OSC_Query_Item ptp_tree [] = {
	// read-write
	OSC_QUERY_ITEM_METHOD("enabled", "Enable/disable", _ptp_enabled, config_boolean_args),
	OSC_QUERY_ITEM_METHOD("multiplier", "Delay request rate multiplier", _ptp_multiplier, ptp_multiplier_args),
	OSC_QUERY_ITEM_METHOD("offset_stiffness", "Stiffness of offset filter", _ptp_offset_stiffness, ptp_stiffness_args),
	OSC_QUERY_ITEM_METHOD("delay_stiffness", "Stiffness of delay filter", _ptp_delay_stiffness, ptp_stiffness_args),

	// read-only
	OSC_QUERY_ITEM_METHOD("offset", "Sync offset", _ptp_offset, ptp_offset_args),
	OSC_QUERY_ITEM_METHOD("delay", "Sync roundtrip delay", _ptp_delay, ptp_delay_args)
};

D ptp/ptp_private.h => ptp/ptp_private.h +0 -113
@@ 1,113 0,0 @@
/*
 * Copyright (c) 2015 Hanspeter Portner (dev@open-music-kontrollers.ch)
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the Artistic License 2.0 as published by
 * The Perl Foundation.
 *
 * This source is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * Artistic License 2.0 for more details.
 *
 * You should have received a copy of the Artistic License 2.0
 * along the source as a COPYING file. If not, obtain it from
 * http://www.perlfoundation.org/artistic_license_2_0.
 */

#ifndef _PTP_PRIVATE_H_
#define _PTP_PRIVATE_H_

#include <stdint.h>

#include <ptp.h>
#include <armfix.h>

typedef struct _PTP_Timestamp PTP_Timestamp;
typedef struct _PTP_Request PTP_Request;
#define PTP_Sync PTP_Request
#define PTP_Follow_Up PTP_Request
typedef struct _PTP_Response PTP_Response;
typedef struct _PTP_Announce PTP_Announce;

struct _PTP_Timestamp {
	uint16_t epoch;
	uint32_t sec;
	uint32_t nsec;
} __attribute((packed));

struct _PTP_Request {
	uint8_t message_id;
	uint8_t version;
	uint16_t message_length;
	uint8_t subdomain_number;
	uint8_t UNUSED_2;
	uint16_t flags;
	uint64_t correction; // 6.2 ns
	uint32_t UNUSED_3;
	uint64_t clock_identity;
	uint16_t source_port_id;
	uint16_t sequence_id;
	uint8_t control;
	uint8_t log_message_interval;
	PTP_Timestamp timestamp;
} __attribute((packed));

struct _PTP_Response {
	PTP_Request req;
	uint64_t requesting_clock_identity;
	uint16_t requesting_source_port_id;
} __attribute((packed));

struct _PTP_Announce {
	PTP_Request req;
	uint16_t origin_current_utc_offset;
	uint8_t UNUSED_4;
	uint8_t priotity_1;
	uint8_t grandmaster_clock_class;
	uint8_t grandmaster_clock_accuracy;
	uint16_t gra