etherlabmaster: master/canopen.c@8669ecb68cb0 (annotated)

64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	1	/******************************************************************************
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	2	*
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	3	* c a n o p e n . c
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	4	*
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	5	* CANopen over EtherCAT
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	6	*
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	7	* $Id$
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	8	*
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	9	*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	10
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	11	#include <linux/delay.h>
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	12	#include <linux/slab.h>
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	13	#include <linux/module.h>
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	14
77 677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	15	#include "../include/EtherCAT_si.h"
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	16	#include "master.h"
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	17
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	18	/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	19
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	20	/**
9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	21	Schreibt ein CANopen-SDO (service data object).
9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	22	*/
9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	23
77 677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	24	int EtherCAT_rt_canopen_sdo_write(ec_slave_t slave, /< EtherCAT-Slave /
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	25	uint16_t sdo_index, /*< SDO-Index /
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	26	uint8_t sdo_subindex, /*< SDO-Subindex /
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	27	uint32_t value, /*< Neuer Wert /
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	28	size_t size /*< Größe des Datenfeldes /
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	29	)
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	30	{
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	31	unsigned char data[0xF6];
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	32	ec_frame_t frame;
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	33	unsigned int tries_left, i;
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	34	ec_master_t *master;
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	35
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	36	memset(data, 0x00, 0xF6);
9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	37
9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	38	master = slave->master;
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	39
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	40	if (size == 0 \|\| size > 4) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	41	EC_ERR("Illegal SDO data size: %i!\n", size);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	42	return -1;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	43	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	44
77 677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	45	EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	46	EC_WRITE_U16(data + 2, slave->station_address); // Station address
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	47	EC_WRITE_U8 (data + 4, 0x00); // Channel & priority
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	48	EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	49	EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request)
77 677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	50	EC_WRITE_U8 (data + 8, 0x13 \| ((4 - size) << 2)); // Spec., exp., init.
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	51	EC_WRITE_U16(data + 9, sdo_index);
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	52	EC_WRITE_U8 (data + 11, sdo_subindex);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	53
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	54	for (i = 0; i < size; i++) {
77 677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	55	EC_WRITE_U8(data + 12 + i, value & 0xFF);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	56	value >>= 8;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	57	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	58
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	59	ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6,
9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	60	data);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	61
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	62	if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	63
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	64	if (unlikely(frame.working_counter != 1)) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	65	EC_ERR("Mailbox send - Slave %i did not respond!\n",
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	66	slave->ring_position);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	67	return -1;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	68	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	69
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	70	// Read "written bit" of Sync-Manager
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	71
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	72	tries_left = 10;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	73	while (tries_left)
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	74	{
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	75	ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	76
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	77	if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	78
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	79	if (unlikely(frame.working_counter != 1)) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	80	EC_ERR("Mailbox check - Slave %i did not respond!\n",
b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	81	slave->ring_position);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	82	return -1;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	83	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	84
77 677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	85	if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	86	break;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	87	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	88
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	89	udelay(1000);
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	90	tries_left--;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	91	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	92
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	93	if (!tries_left) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	94	EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	95	return -1;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	96	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	97
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	98	ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	99
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	100	if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	101
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	102	if (unlikely(frame.working_counter != 1)) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	103	EC_ERR("Mailbox receive - Slave %i did not respond!\n",
b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	104	slave->ring_position);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	105	return -1;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	106	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	107
77 677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	108	if (EC_READ_U8 (frame.data + 5) != 0x03 \|\| // COE
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	109	EC_READ_U16(frame.data + 6) != 0x3000 \|\| // SDO response
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	110	EC_READ_U8 (frame.data + 8) >> 5 != 0x03 \|\| // Download response
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	111	EC_READ_U16(frame.data + 9) != sdo_index \|\| // Index
677967864795 EC_READ/WRITE-Makros verwenden Makros aud asm/byteorder.h und werden konsequent verwendet. Florian Pose <fp@igh-essen.com> parents: 73 diff changeset	112	EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	113	{
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	114	EC_ERR("Illegal mailbox response at slave %i!\n",
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	115	slave->ring_position);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	116	return -1;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	117	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	118
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	119	return 0;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	120	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	121
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	122	/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	123
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	124	/**
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	125	Schreibt ein CANopen-SDO (service data object).
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	126	*/
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	127
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	128	int EtherCAT_rt_canopen_sdo_read(ec_slave_t slave, /< EtherCAT-Slave /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	129	uint16_t sdo_index, /*< SDO-Index /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	130	uint8_t sdo_subindex, /*< SDO-Subindex /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	131	uint32_t value /< Speicher für gel. Wert /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	132	)
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	133	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	134	unsigned char data[0xF6];
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	135	ec_frame_t frame;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	136	unsigned int tries_left;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	137	ec_master_t *master;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	138
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	139	memset(data, 0x00, 0xF6);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	140
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	141	master = slave->master;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	142
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	143	EC_WRITE_U16(data, 0x0006); // Length of the Mailbox service data
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	144	EC_WRITE_U16(data + 2, slave->station_address); // Station address
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	145	EC_WRITE_U8 (data + 4, 0x00); // Channel & priority
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	146	EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	147	EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request)
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	148	EC_WRITE_U8 (data + 8, 0x1 << 1 \| 0x2 << 5); // Exp., Upload request
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	149	EC_WRITE_U16(data + 9, sdo_index);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	150	EC_WRITE_U8 (data + 11, sdo_subindex);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	151
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	152	ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	153	data);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	154
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	155	if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	156
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	157	if (unlikely(frame.working_counter != 1)) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	158	EC_ERR("Mailbox send - Slave %i did not respond!\n",
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	159	slave->ring_position);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	160	return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	161	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	162
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	163	// Read "written bit" of Sync-Manager
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	164
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	165	tries_left = 10;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	166	while (tries_left)
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	167	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	168	ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	169
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	170	if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	171
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	172	if (unlikely(frame.working_counter != 1)) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	173	EC_ERR("Mailbox check - Slave %i did not respond!\n",
b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	174	slave->ring_position);
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	175	return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	176	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	177
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	178	if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	179	break;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	180	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	181
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	182	udelay(1000);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	183	tries_left--;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	184	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	185
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	186	if (!tries_left) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	187	EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position);
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	188	return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	189	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	190
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	191	ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	192
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	193	if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	194
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	195	if (unlikely(frame.working_counter != 1)) {
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	196	EC_ERR("Mailbox receive - Slave %i did not respond!\n",
b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	197	slave->ring_position);
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	198	return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	199	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	200
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	201	if (EC_READ_U8 (frame.data + 5) != 0x03 \|\| // COE
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	202	EC_READ_U16(frame.data + 6) != 0x3000 \|\| // SDO response
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	203	EC_READ_U8 (frame.data + 8) >> 5 != 0x02 \|\| // Upload response
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	204	EC_READ_U16(frame.data + 9) != sdo_index \|\| // Index
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	205	EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	206	{
84 b4ae98855cea printk durch Makros ersetzt. Florian Pose <fp@igh-essen.com> parents: 80 diff changeset	207	EC_ERR("Illegal mailbox response at slave %i!\n",
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	208	slave->ring_position);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	209	return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	210	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	211
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	212	*value = EC_READ_U32(frame.data + 12);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	213
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	214	return 0;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	215	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	216
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	217	/*****************************************************************************/
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	218
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	219	int EtherCAT_rt_canopen_sdo_addr_write(ec_master_t *master,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	220	/*< EtherCAT-Master /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	221	const char *addr,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	222	/*< Addresse, siehe ec_address() /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	223	uint16_t index,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	224	/*< SDO-Index /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	225	uint8_t subindex,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	226	/*< SDO-Subindex /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	227	uint32_t value,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	228	/*< Neuer Wert /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	229	size_t size
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	230	/*< Größe des Datenfeldes /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	231	)
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	232	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	233	ec_slave_t *slave;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	234	if (!(slave = ec_address(master, addr))) return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	235	return EtherCAT_rt_canopen_sdo_write(slave, index, subindex, value, size);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	236	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	237
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	238	/*****************************************************************************/
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	239
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	240	int EtherCAT_rt_canopen_sdo_addr_read(ec_master_t *master,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	241	/*< EtherCAT-Slave /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	242	const char *addr,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	243	/*< Addresse, siehe ec_address() /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	244	uint16_t index,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	245	/*< SDO-Index /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	246	uint8_t subindex,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	247	/*< SDO-Subindex /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	248	uint32_t *value
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	249	/*< Speicher für gel. Wert /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	250	)
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	251	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	252	ec_slave_t *slave;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	253	if (!(slave = ec_address(master, addr))) return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	254	return EtherCAT_rt_canopen_sdo_read(slave, index, subindex, value);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	255	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	256
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	257	/*****************************************************************************/
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	258
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	259	EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_write);
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	260	EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_read);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	261	EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_addr_write);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	262	EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_addr_read);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	263
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	264	/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	265
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	266	/* Emacs-Konfiguration
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	267	;;; Local Variables: ***
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	268	;;; c-basic-offset:4 ***
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	269	;;; End: ***
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	270	*/

author	Florian Pose <fp@igh-essen.com>
	Sat, 25 Feb 2006 14:25:06 +0000
changeset 86	8669ecb68cb0
parent 84	b4ae98855cea
child 89	e91ef35c36db
permissions	-rw-r--r--