block_ec.c 3.13 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
/*
Jeroen Vreeken's avatar
Jeroen Vreeken committed
2
3
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009 - 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2009 - 2013
Jeroen Vreeken's avatar
Jeroen Vreeken committed
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

	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>
25
#include <controller/controller_bus.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
26
27
28

#include "block_ec.h"
#include "ec.h"
29
#include "log.h"
Jeroen Vreeken's avatar
Jeroen Vreeken committed
30

Jeroen Vreeken's avatar
Jeroen Vreeken committed
31

32
33
34
35
struct controller_block_private {
	bool pdo_data;
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
36
37
static void calculate(struct controller_block *ec)
{
38
39
40
	if (ec_rx_pdo()) {
		ec->private->pdo_data = true;
	}
41
42
43
44
45
}

static void calculate_tx(struct controller_block *ec)
{
	ec_tx_pdo();
Jeroen Vreeken's avatar
Jeroen Vreeken committed
46
47
48
	ec_datagram_sample();
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
49
static void param_set(struct controller_block *ec, int param, va_list val)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
50
51
52
53
54
55
56
57
58
59
60
61
{
	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 },
};

62
63
64
65
66
static struct controller_block_outterm_list outterms[] = {
	{ "pdo_data",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, pdo_data)  },
	{ NULL },
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
67
68
struct controller_block * block_ec_create(char *name, va_list ap)
{
69
70
	struct controller_block *ec, *ec_tx;
	struct controller_bus *bus;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
71
72
73
74
	int ret;
	char *ifname;
	int nr_slaves;
	int single_cycle;
75
	char name_tx[strlen(name)+strlen("_pdo_tx0")];
Jeroen Vreeken's avatar
Jeroen Vreeken committed
76
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
77
78
79
80
81
82
83
	/* For now we only support a single ethercat interface,
	   but we allow multiple inits
	 */
	if (ec_block) {
		return ec_block;
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
84
85
86
87
	ifname = va_arg(ap, char *);
	nr_slaves = va_arg(ap, int);
	single_cycle = va_arg(ap, int);
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
88
	ret = ec_init(ifname, nr_slaves, single_cycle);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
89
90
91
92
93

	if (ret != 0)
		return NULL;


94
	sprintf(name_tx, "%s_pdo_tx", name);
95
96
	ec = controller_block_alloc("ec", name, 
	    sizeof(struct controller_block_private));
Jeroen Vreeken's avatar
Jeroen Vreeken committed
97
98
	if (!ec)
		goto err_ec;
99
100
101
102
	ec_tx = controller_block_alloc("ec_tx", name_tx, 0);
	if (!ec_tx)
		goto err_ec_tx;

103
104
105
	if (controller_block_outterm_list_init(ec, outterms))
		goto err_outputs;	

Jeroen Vreeken's avatar
Jeroen Vreeken committed
106
107
	bus = controller_bus_create("ethercat", name, ec, 
	    sizeof(struct controller_bus_private));
108
109
110
111
	if (!bus)
		goto err_bus;

	controller_bus_output_set(bus, ec_tx);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
112

Jeroen Vreeken's avatar
Jeroen Vreeken committed
113
114
115
	bus->private->logical_rx = EC_LOGICAL_RX_START;
	bus->private->logical_tx = EC_LOGICAL_TX_START;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
116
	ec->calculate = calculate;
117
	ec_tx->calculate = calculate_tx;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
118
119
120
121

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

Jeroen Vreeken's avatar
Jeroen Vreeken committed
122
	ec->param_get = NULL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
123
	ec->param_set = param_set;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
124
125

	controller_block_add(ec);
126
	controller_block_add(ec_tx);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
127
128

	ec_block = ec;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
129
	ec_bus = bus;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130

Jeroen Vreeken's avatar
Jeroen Vreeken committed
131
132
	return ec;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
133
err_params:
134
135
136
	free(bus);
err_bus:
	free(ec_tx);
137
err_outputs:
138
139
err_ec_tx:
	free(ec->private);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
140
	free(ec);
141
err_ec:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
142
143
	return NULL;
}