master/canopen.c
changeset 409 bc4c852e1b93
child 419 cf724fc82a00
equal deleted inserted replaced
408:0eda82d1a8c1 409:bc4c852e1b93
       
     1 /******************************************************************************
       
     2  *
       
     3  *  $Id$
       
     4  *
       
     5  *  Copyright (C) 2006  Florian Pose, Ingenieurgemeinschaft IgH
       
     6  *
       
     7  *  This file is part of the IgH EtherCAT Master.
       
     8  *
       
     9  *  The IgH EtherCAT Master is free software; you can redistribute it
       
    10  *  and/or modify it under the terms of the GNU General Public License
       
    11  *  as published by the Free Software Foundation; either version 2 of the
       
    12  *  License, or (at your option) any later version.
       
    13  *
       
    14  *  The IgH EtherCAT Master is distributed in the hope that it will be
       
    15  *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
       
    16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       
    17  *  GNU General Public License for more details.
       
    18  *
       
    19  *  You should have received a copy of the GNU General Public License
       
    20  *  along with the IgH EtherCAT Master; if not, write to the Free Software
       
    21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
       
    22  *
       
    23  *  The right to use EtherCAT Technology is granted and comes free of
       
    24  *  charge under condition of compatibility of product made by
       
    25  *  Licensee. People intending to distribute/sell products based on the
       
    26  *  code, have to sign an agreement to guarantee that products using
       
    27  *  software based on IgH EtherCAT master stay compatible with the actual
       
    28  *  EtherCAT specification (which are released themselves as an open
       
    29  *  standard) as the (only) precondition to have the right to use EtherCAT
       
    30  *  Technology, IP and trade marks.
       
    31  *
       
    32  *****************************************************************************/
       
    33 
       
    34 /**
       
    35    \file
       
    36    Canopen-over-EtherCAT functions.
       
    37 */
       
    38 
       
    39 /*****************************************************************************/
       
    40 
       
    41 #include <linux/module.h>
       
    42 
       
    43 #include "canopen.h"
       
    44 
       
    45 /*****************************************************************************/
       
    46 
       
    47 ssize_t ec_show_sdo_attribute(struct kobject *, struct attribute *, char *);
       
    48 ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *,
       
    49                                     char *);
       
    50 void ec_sdo_clear(struct kobject *);
       
    51 void ec_sdo_entry_clear(struct kobject *);
       
    52 
       
    53 /*****************************************************************************/
       
    54 
       
    55 /** \cond */
       
    56 
       
    57 EC_SYSFS_READ_ATTR(info);
       
    58 
       
    59 static struct attribute *sdo_def_attrs[] = {
       
    60     &attr_info,
       
    61     NULL,
       
    62 };
       
    63 
       
    64 static struct sysfs_ops sdo_sysfs_ops = {
       
    65     .show = &ec_show_sdo_attribute,
       
    66     .store = NULL
       
    67 };
       
    68 
       
    69 static struct kobj_type ktype_ec_sdo = {
       
    70     .release = ec_sdo_clear,
       
    71     .sysfs_ops = &sdo_sysfs_ops,
       
    72     .default_attrs = sdo_def_attrs
       
    73 };
       
    74 
       
    75 static struct attribute *sdo_entry_def_attrs[] = {
       
    76     &attr_info,
       
    77     NULL,
       
    78 };
       
    79 
       
    80 static struct sysfs_ops sdo_entry_sysfs_ops = {
       
    81     .show = &ec_show_sdo_entry_attribute,
       
    82     .store = NULL
       
    83 };
       
    84 
       
    85 static struct kobj_type ktype_ec_sdo_entry = {
       
    86     .release = ec_sdo_entry_clear,
       
    87     .sysfs_ops = &sdo_entry_sysfs_ops,
       
    88     .default_attrs = sdo_entry_def_attrs
       
    89 };
       
    90 
       
    91 /** \endcond */
       
    92 
       
    93 /*****************************************************************************/
       
    94 
       
    95 /**
       
    96    SDO constructor.
       
    97 */
       
    98 
       
    99 int ec_sdo_init(ec_sdo_t *sdo, /**< SDO */
       
   100                 uint16_t index, /**< SDO index */
       
   101                 ec_slave_t *slave /**< parent slave */
       
   102                 )
       
   103 {
       
   104     sdo->index = index;
       
   105     sdo->object_code = 0x00;
       
   106     sdo->name = NULL;
       
   107     sdo->subindices = 0;
       
   108     INIT_LIST_HEAD(&sdo->entries);
       
   109 
       
   110     // init kobject and add it to the hierarchy
       
   111     memset(&sdo->kobj, 0x00, sizeof(struct kobject));
       
   112     kobject_init(&sdo->kobj);
       
   113     sdo->kobj.ktype = &ktype_ec_sdo;
       
   114     sdo->kobj.parent = &slave->kobj;
       
   115     if (kobject_set_name(&sdo->kobj, "sdo%4X", sdo->index)) {
       
   116         EC_ERR("Failed to set kobj name.\n");
       
   117         return -1;
       
   118     }
       
   119 
       
   120     return 0;
       
   121 }
       
   122 
       
   123 /*****************************************************************************/
       
   124 
       
   125 /**
       
   126    SDO destructor.
       
   127 */
       
   128 
       
   129 void ec_sdo_clear(struct kobject *kobj /**< SDO's kobject */)
       
   130 {
       
   131     ec_sdo_t *sdo = container_of(kobj, ec_sdo_t, kobj);
       
   132     ec_sdo_entry_t *entry, *next;
       
   133 
       
   134     // free all entries
       
   135     list_for_each_entry_safe(entry, next, &sdo->entries, list) {
       
   136         list_del(&entry->list);
       
   137         kobject_del(&entry->kobj);
       
   138         kobject_put(&entry->kobj);
       
   139     }
       
   140 
       
   141     if (sdo->name) kfree(sdo->name);
       
   142 
       
   143     kfree(sdo);
       
   144 }
       
   145 
       
   146 /*****************************************************************************/
       
   147 
       
   148 ssize_t ec_sdo_info(ec_sdo_t *sdo, /**< SDO */
       
   149                     char *buffer /**< target buffer */
       
   150                     )
       
   151 {
       
   152     off_t off = 0;
       
   153 
       
   154     off += sprintf(buffer + off, "Index: 0x%04X\n", sdo->index);
       
   155     off += sprintf(buffer + off, "Name: %s\n", sdo->name ? sdo->name : "");
       
   156     off += sprintf(buffer + off, "Subindices: %i\n", sdo->subindices);
       
   157 
       
   158     return off;
       
   159 }
       
   160 
       
   161 /*****************************************************************************/
       
   162 
       
   163 ssize_t ec_show_sdo_attribute(struct kobject *kobj, /**< kobject */
       
   164                               struct attribute *attr,
       
   165                               char *buffer
       
   166                               )
       
   167 {
       
   168     ec_sdo_t *sdo = container_of(kobj, ec_sdo_t, kobj);
       
   169 
       
   170     if (attr == &attr_info) {
       
   171         return ec_sdo_info(sdo, buffer);
       
   172     }
       
   173 
       
   174     return 0;
       
   175 }
       
   176 
       
   177 /*****************************************************************************/
       
   178 
       
   179 /**
       
   180    SDO entry constructor.
       
   181 */
       
   182 
       
   183 int ec_sdo_entry_init(ec_sdo_entry_t *entry, /**< SDO entry */
       
   184                       uint8_t subindex, /**< SDO entry subindex */
       
   185                       ec_sdo_t *sdo /**< parent SDO */
       
   186                       )
       
   187 {
       
   188     entry->subindex = subindex;
       
   189     entry->data_type = 0x0000;
       
   190     entry->bit_length = 0;
       
   191     entry->description = NULL;
       
   192 
       
   193     // init kobject and add it to the hierarchy
       
   194     memset(&entry->kobj, 0x00, sizeof(struct kobject));
       
   195     kobject_init(&entry->kobj);
       
   196     entry->kobj.ktype = &ktype_ec_sdo_entry;
       
   197     entry->kobj.parent = &sdo->kobj;
       
   198     if (kobject_set_name(&entry->kobj, "entry%02X", entry->subindex)) {
       
   199         EC_ERR("Failed to set kobj name.\n");
       
   200         return -1;
       
   201     }
       
   202 
       
   203     return 0;
       
   204 }
       
   205 
       
   206 /*****************************************************************************/
       
   207 
       
   208 /**
       
   209    SDO destructor.
       
   210 */
       
   211 
       
   212 void ec_sdo_entry_clear(struct kobject *kobj /**< SDO entry's kobject */)
       
   213 {
       
   214     ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj);
       
   215 
       
   216     if (entry->description) kfree(entry->description);
       
   217 
       
   218     kfree(entry);
       
   219 }
       
   220 
       
   221 /*****************************************************************************/
       
   222 
       
   223 ssize_t ec_sdo_entry_info(ec_sdo_entry_t *entry, /**< SDO entry */
       
   224                           char *buffer /**< target buffer */
       
   225                           )
       
   226 {
       
   227     off_t off = 0;
       
   228 
       
   229     off += sprintf(buffer + off, "Subindex: 0x%02X\n", entry->subindex);
       
   230     off += sprintf(buffer + off, "Description: %s\n",
       
   231                    entry->description ? entry->description : "");
       
   232     off += sprintf(buffer + off, "Data type: 0x%04X\n", entry->data_type);
       
   233     off += sprintf(buffer + off, "Bit length: %i\n", entry->bit_length);
       
   234 
       
   235     return off;
       
   236 }
       
   237 
       
   238 /*****************************************************************************/
       
   239 
       
   240 ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */
       
   241                                     struct attribute *attr,
       
   242                                     char *buffer
       
   243                                     )
       
   244 {
       
   245     ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj);
       
   246 
       
   247     if (attr == &attr_info) {
       
   248         return ec_sdo_entry_info(entry, buffer);
       
   249     }
       
   250 
       
   251     return 0;
       
   252 }
       
   253 
       
   254 /*****************************************************************************/
       
   255 
       
   256 #if 0
       
   257 int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT slave */
       
   258                         uint16_t sdo_index, /**< SDO index */
       
   259                         uint8_t sdo_subindex, /**< SDO subindex */
       
   260                         uint8_t *target, /**< memory for value */
       
   261                         size_t *size /**< target memory size */
       
   262                         )
       
   263 {
       
   264     uint8_t *data;
       
   265     size_t rec_size, data_size;
       
   266     uint32_t complete_size;
       
   267     ec_datagram_t datagram;
       
   268 
       
   269     ec_datagram_init(&datagram);
       
   270 
       
   271     if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6)))
       
   272         goto err;
       
   273 
       
   274     EC_WRITE_U16(data, 0x2 << 12); // SDO request
       
   275     EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
       
   276     EC_WRITE_U16(data + 3, sdo_index);
       
   277     EC_WRITE_U8 (data + 5, sdo_subindex);
       
   278 
       
   279     if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size)))
       
   280         goto err;
       
   281 
       
   282     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
       
   283         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
       
   284         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
       
   285                sdo_index, sdo_subindex, slave->ring_position);
       
   286         ec_canopen_abort_msg(EC_READ_U32(data + 6));
       
   287         goto err;
       
   288     }
       
   289 
       
   290     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
       
   291         EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response
       
   292         EC_READ_U16(data + 3) != sdo_index || // index
       
   293         EC_READ_U8 (data + 5) != sdo_subindex) { // subindex
       
   294         EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo_index, sdo_subindex);
       
   295         EC_ERR("Invalid SDO upload response at slave %i!\n",
       
   296                slave->ring_position);
       
   297         ec_print_data(data, rec_size);
       
   298         goto err;
       
   299     }
       
   300 
       
   301     if (rec_size < 10) {
       
   302         EC_ERR("Received currupted SDO upload response!\n");
       
   303         ec_print_data(data, rec_size);
       
   304         goto err;
       
   305     }
       
   306 
       
   307     if ((complete_size = EC_READ_U32(data + 6)) > *size) {
       
   308         EC_ERR("SDO data does not fit into buffer (%i / %i)!\n",
       
   309                complete_size, *size);
       
   310         goto err;
       
   311     }
       
   312 
       
   313     data_size = rec_size - 10;
       
   314 
       
   315     if (data_size != complete_size) {
       
   316         EC_ERR("SDO data incomplete - Fragmenting not implemented.\n");
       
   317         goto err;
       
   318     }
       
   319 
       
   320     memcpy(target, data + 10, data_size);
       
   321 
       
   322     ec_datagram_clear(&datagram);
       
   323     return 0;
       
   324  err:
       
   325     ec_datagram_clear(&datagram);
       
   326     return -1;
       
   327 }
       
   328 #endif
       
   329 
       
   330 /*****************************************************************************/