etherlabmaster: master/canopen.c@e8b76a509bc9 (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) {
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	41	printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size);
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)) {
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	65	printk(KERN_ERR "EtherCAT: 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)) {
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	80	printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not"
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	81	" respond!\n", 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) {
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	94	printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n",
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	95	slave->ring_position);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	96	return -1;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	97	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	98
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	99	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	100
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	101	if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	102
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	103	if (unlikely(frame.working_counter != 1)) {
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	104	printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not"
73 9f4ea66d89a3 Dynamische FMMU-Konfiguration, zwei Kopieroperationen eingespart, Einr?ckungen angepasst. Florian Pose <fp@igh-essen.com> parents: 66 diff changeset	105	" respond!\n", slave->ring_position);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	106	return -1;
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	107	}
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	108
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	109	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	110	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	111	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	112	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	113	EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	114	{
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	115	printk(KERN_ERR "EtherCAT: 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	116	slave->ring_position);
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	117	return -1;
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
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	120	return 0;
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	/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	124
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	125	/**
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	126	Schreibt ein CANopen-SDO (service data object).
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
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	129	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	130	uint16_t sdo_index, /*< SDO-Index /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	131	uint8_t sdo_subindex, /*< SDO-Subindex /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	132	uint32_t value /< Speicher für gel. Wert /
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	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	135	unsigned char data[0xF6];
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	136	ec_frame_t frame;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	137	unsigned int tries_left;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	138	ec_master_t *master;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	139
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	140	memset(data, 0x00, 0xF6);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	141
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	142	master = slave->master;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	143
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	144	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	145	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	146	EC_WRITE_U8 (data + 4, 0x00); // Channel & priority
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	147	EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	148	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	149	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	150	EC_WRITE_U16(data + 9, sdo_index);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	151	EC_WRITE_U8 (data + 11, sdo_subindex);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	152
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	153	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	154	data);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	155
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	156	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	157
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	158	if (unlikely(frame.working_counter != 1)) {
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	159	printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n",
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	160	slave->ring_position);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	161	return -1;
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
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	164	// Read "written bit" of Sync-Manager
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	165
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	166	tries_left = 10;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	167	while (tries_left)
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	168	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	169	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	170
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	171	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	172
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	173	if (unlikely(frame.working_counter != 1)) {
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	174	printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not"
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	175	" respond!\n", slave->ring_position);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	176	return -1;
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
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	179	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	180	break;
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
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	183	udelay(1000);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	184	tries_left--;
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
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	187	if (!tries_left) {
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	188	printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n",
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	189	slave->ring_position);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	190	return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	191	}
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	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	194
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	195	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	196
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	197	if (unlikely(frame.working_counter != 1)) {
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	198	printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not"
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	199	" respond!\n", slave->ring_position);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	200	return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	201	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	202
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	203	if (EC_READ_U8 (frame.data + 5) != 0x03 \|\| // COE
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	204	EC_READ_U16(frame.data + 6) != 0x3000 \|\| // SDO response
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	205	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	206	EC_READ_U16(frame.data + 9) != sdo_index \|\| // Index
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	207	EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	208	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	209	printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n",
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	210	slave->ring_position);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	211	return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	212	}
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	*value = EC_READ_U32(frame.data + 12);
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	return 0;
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	/*****************************************************************************/
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	220
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	221	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	222	/*< EtherCAT-Master /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	223	const char *addr,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	224	/*< Addresse, siehe ec_address() /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	225	uint16_t index,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	226	/*< SDO-Index /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	227	uint8_t subindex,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	228	/*< SDO-Subindex /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	229	uint32_t value,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	230	/*< Neuer Wert /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	231	size_t size
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	232	/*< Größe des Datenfeldes /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	233	)
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	234	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	235	ec_slave_t *slave;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	236	if (!(slave = ec_address(master, addr))) return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	237	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	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	/*****************************************************************************/
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	241
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	242	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	243	/*< EtherCAT-Slave /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	244	const char *addr,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	245	/*< Addresse, siehe ec_address() /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	246	uint16_t index,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	247	/*< SDO-Index /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	248	uint8_t subindex,
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	249	/*< SDO-Subindex /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	250	uint32_t *value
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	251	/*< Speicher für gel. Wert /
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	252	)
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	253	{
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	254	ec_slave_t *slave;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	255	if (!(slave = ec_address(master, addr))) return -1;
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	256	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	257	}
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	258
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	259	/*****************************************************************************/
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	260
64 ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	261	EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_write);
80 8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	262	EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_read);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	263	EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_addr_write);
8cd08d73b560 CANopen SDO read implementiert. Florian Pose <fp@igh-essen.com> parents: 77 diff changeset	264	EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_addr_read);
64 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	/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	267
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	268	/* Emacs-Konfiguration
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	269	;;; Local Variables: ***
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	270	;;; c-basic-offset:4 ***
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	271	;;; End: ***
ea6ccf12c612 CANopen over EtherCAT. Florian Pose <fp@igh-essen.com> parents: diff changeset	272	*/

author	Florian Pose <fp@igh-essen.com>
	Fri, 24 Feb 2006 14:09:51 +0000
changeset 83	e8b76a509bc9
parent 80	8cd08d73b560
child 84	b4ae98855cea
permissions	-rw-r--r--