block_ec.c

/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009 - 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2009 - 2013

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program 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
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

 */

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

#include <controller/controller_block.h>
#include <controller/controller_bus.h>

#include "block_ec.h"
#include "ec.h"
#include "log.h"


struct controller_block_private {
	bool pdo_data;
};

static void calculate(struct controller_block *ec)
{
	if (ec_rx_pdo()) {
		ec->private->pdo_data = true;
	}
}

static void calculate_tx(struct controller_block *ec)
{
	ec_tx_pdo();
	ec_datagram_sample();
}

static void param_set(struct controller_block *ec, int param, va_list val)
{
	int enabled = va_arg(val, int);

	ec_rx_pdo_enable(enabled);
	ec_tx_pdo_enable(enabled);
}

static struct controller_block_param_list params[] = {
	{ "pdo_enabled", true },
	{ NULL },
};

static struct controller_block_outterm_list outterms[] = {
	{ "pdo_data",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, pdo_data)  },
	{ NULL },
};

struct controller_block * block_ec_create(char *name, va_list ap)
{
	struct controller_block *ec, *ec_tx;
	struct controller_bus *bus;
	int ret;
	char *ifname;
	int nr_slaves;
	int single_cycle;
	char name_tx[strlen(name)+strlen("_pdo_tx0")];
	
	/* For now we only support a single ethercat interface,
	   but we allow multiple inits
	 */
	if (ec_block) {
		return ec_block;
	}

	ifname = va_arg(ap, char *);
	nr_slaves = va_arg(ap, int);
	single_cycle = va_arg(ap, int);
	
	ret = ec_init(ifname, nr_slaves, single_cycle);

	if (ret != 0)
		return NULL;


	sprintf(name_tx, "%s_pdo_tx", name);
	ec = controller_block_alloc("ec", name, 
	    sizeof(struct controller_block_private));
	if (!ec)
		goto err_ec;
	ec_tx = controller_block_alloc("ec_tx", name_tx, 0);
	if (!ec_tx)
		goto err_ec_tx;

	if (controller_block_outterm_list_init(ec, outterms))
		goto err_outputs;	

	bus = controller_bus_create("ethercat", name, ec, 
	    sizeof(struct controller_bus_private));
	if (!bus)
		goto err_bus;

	controller_bus_output_set(bus, ec_tx);

	bus->private->logical_rx = EC_LOGICAL_RX_START;
	bus->private->logical_tx = EC_LOGICAL_TX_START;

	ec->calculate = calculate;
	ec_tx->calculate = calculate_tx;

	if (controller_block_param_list_init(ec, params))
		goto err_params;

	ec->param_get = NULL;
	ec->param_set = param_set;

	controller_block_add(ec);
	controller_block_add(ec_tx);

	ec_block = ec;
	ec_bus = bus;

	return ec;

err_params:
	free(bus);
err_bus:
	free(ec_tx);
err_outputs:
err_ec_tx:
	free(ec->private);
	free(ec);
err_ec:
	return NULL;
}