master/canopen.c
changeset 430 74754f45d5fa
parent 419 cf724fc82a00
child 441 ffa13db95e10
equal deleted inserted replaced
429:b21b000e88e3 430:74754f45d5fa
    39 /*****************************************************************************/
    39 /*****************************************************************************/
    40 
    40 
    41 #include <linux/module.h>
    41 #include <linux/module.h>
    42 
    42 
    43 #include "canopen.h"
    43 #include "canopen.h"
       
    44 #include "master.h"
    44 
    45 
    45 /*****************************************************************************/
    46 /*****************************************************************************/
    46 
    47 
    47 ssize_t ec_show_sdo_attribute(struct kobject *, struct attribute *, char *);
    48 ssize_t ec_show_sdo_attribute(struct kobject *, struct attribute *, char *);
    48 ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *,
    49 ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *,
    49                                     char *);
    50                                     char *);
    50 void ec_sdo_clear(struct kobject *);
    51 void ec_sdo_clear(struct kobject *);
    51 void ec_sdo_entry_clear(struct kobject *);
    52 void ec_sdo_entry_clear(struct kobject *);
    52 
    53 
       
    54 void ec_sdo_request_init_read(ec_sdo_request_t *, ec_sdo_t *,
       
    55                               ec_sdo_entry_t *);
       
    56 void ec_sdo_request_clear(ec_sdo_request_t *);
       
    57 
    53 /*****************************************************************************/
    58 /*****************************************************************************/
    54 
    59 
    55 /** \cond */
    60 /** \cond */
    56 
    61 
    57 EC_SYSFS_READ_ATTR(info);
    62 EC_SYSFS_READ_ATTR(info);
       
    63 EC_SYSFS_READ_ATTR(value);
    58 
    64 
    59 static struct attribute *sdo_def_attrs[] = {
    65 static struct attribute *sdo_def_attrs[] = {
    60     &attr_info,
    66     &attr_info,
    61     NULL,
    67     NULL,
    62 };
    68 };
    72     .default_attrs = sdo_def_attrs
    78     .default_attrs = sdo_def_attrs
    73 };
    79 };
    74 
    80 
    75 static struct attribute *sdo_entry_def_attrs[] = {
    81 static struct attribute *sdo_entry_def_attrs[] = {
    76     &attr_info,
    82     &attr_info,
       
    83     &attr_value,
    77     NULL,
    84     NULL,
    78 };
    85 };
    79 
    86 
    80 static struct sysfs_ops sdo_entry_sysfs_ops = {
    87 static struct sysfs_ops sdo_entry_sysfs_ops = {
    81     .show = &ec_show_sdo_entry_attribute,
    88     .show = &ec_show_sdo_entry_attribute,
    99 int ec_sdo_init(ec_sdo_t *sdo, /**< SDO */
   106 int ec_sdo_init(ec_sdo_t *sdo, /**< SDO */
   100                 uint16_t index, /**< SDO index */
   107                 uint16_t index, /**< SDO index */
   101                 ec_slave_t *slave /**< parent slave */
   108                 ec_slave_t *slave /**< parent slave */
   102                 )
   109                 )
   103 {
   110 {
       
   111     sdo->slave = slave;
   104     sdo->index = index;
   112     sdo->index = index;
   105     sdo->object_code = 0x00;
   113     sdo->object_code = 0x00;
   106     sdo->name = NULL;
   114     sdo->name = NULL;
   107     sdo->subindices = 0;
   115     sdo->subindices = 0;
   108     INIT_LIST_HEAD(&sdo->entries);
   116     INIT_LIST_HEAD(&sdo->entries);
   183 int ec_sdo_entry_init(ec_sdo_entry_t *entry, /**< SDO entry */
   191 int ec_sdo_entry_init(ec_sdo_entry_t *entry, /**< SDO entry */
   184                       uint8_t subindex, /**< SDO entry subindex */
   192                       uint8_t subindex, /**< SDO entry subindex */
   185                       ec_sdo_t *sdo /**< parent SDO */
   193                       ec_sdo_t *sdo /**< parent SDO */
   186                       )
   194                       )
   187 {
   195 {
       
   196     entry->sdo = sdo;
   188     entry->subindex = subindex;
   197     entry->subindex = subindex;
   189     entry->data_type = 0x0000;
   198     entry->data_type = 0x0000;
   190     entry->bit_length = 0;
   199     entry->bit_length = 0;
   191     entry->description = NULL;
   200     entry->description = NULL;
   192 
   201 
   235     return off;
   244     return off;
   236 }
   245 }
   237 
   246 
   238 /*****************************************************************************/
   247 /*****************************************************************************/
   239 
   248 
       
   249 ssize_t ec_sdo_entry_format_data(ec_sdo_entry_t *entry, /**< SDO entry */
       
   250                                  ec_sdo_request_t *request, /**< SDO request */
       
   251                                  char *buffer /**< target buffer */
       
   252                                  )
       
   253 {
       
   254     off_t off = 0;
       
   255     unsigned int i;
       
   256 
       
   257     if (entry->data_type == 0x0002 && entry->bit_length == 8) { // int8
       
   258         off += sprintf(buffer + off, "%i\n", *((int8_t *) request->data));
       
   259     }
       
   260     else if (entry->data_type == 0x0003 && entry->bit_length == 16) { // int16
       
   261         off += sprintf(buffer + off, "%i\n", *((int16_t *) request->data));
       
   262     }
       
   263     else if (entry->data_type == 0x0004 && entry->bit_length == 32) { // int32
       
   264         off += sprintf(buffer + off, "%i\n", *((int32_t *) request->data));
       
   265     }
       
   266     else if (entry->data_type == 0x0005 && entry->bit_length == 8) { // uint8
       
   267         off += sprintf(buffer + off, "%i\n", *((uint8_t *) request->data));
       
   268     }
       
   269     else if (entry->data_type == 0x0006 && entry->bit_length == 16) { // uint16
       
   270         off += sprintf(buffer + off, "%i\n", *((uint16_t *) request->data));
       
   271     }
       
   272     else if (entry->data_type == 0x0007 && entry->bit_length == 32) { // uint32
       
   273         off += sprintf(buffer + off, "%i\n", *((uint32_t *) request->data));
       
   274     }
       
   275     else if (entry->data_type == 0x0009) { // string
       
   276         off += sprintf(buffer + off, "%s\n", request->data);
       
   277     }
       
   278     else {
       
   279         for (i = 0; i < request->size; i++)
       
   280             off += sprintf(buffer + off, "%02X (%c)\n",
       
   281                            request->data[i], request->data[i]);
       
   282     }
       
   283 
       
   284     return off;
       
   285 }
       
   286 
       
   287 /*****************************************************************************/
       
   288 
       
   289 ssize_t ec_sdo_entry_read_value(ec_sdo_entry_t *entry, /**< SDO entry */
       
   290                                 char *buffer /**< target buffer */
       
   291                                 )
       
   292 {
       
   293     ec_sdo_t *sdo = entry->sdo;
       
   294     ec_master_t *master = sdo->slave->master;
       
   295     off_t off = 0;
       
   296     ec_sdo_request_t request;
       
   297 
       
   298     ec_sdo_request_init_read(&request, sdo, entry);
       
   299     list_add_tail(&request.queue, &master->sdo_requests);
       
   300     if (wait_event_interruptible(master->sdo_wait_queue,
       
   301                                  request.return_code)) {
       
   302         // interrupted by signal
       
   303         list_del_init(&request.queue);
       
   304     }
       
   305 
       
   306     if (request.return_code == 1 && request.data) {
       
   307         off += ec_sdo_entry_format_data(entry, &request, buffer);
       
   308     }
       
   309     else if (request.return_code < 0) {
       
   310         off = -EINVAL;
       
   311     }
       
   312 
       
   313     ec_sdo_request_clear(&request);
       
   314     return off;
       
   315 }
       
   316 
       
   317 /*****************************************************************************/
       
   318 
   240 ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */
   319 ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */
   241                                     struct attribute *attr,
   320                                     struct attribute *attr,
   242                                     char *buffer
   321                                     char *buffer
   243                                     )
   322                                     )
   244 {
   323 {
   245     ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj);
   324     ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj);
   246 
   325 
   247     if (attr == &attr_info) {
   326     if (attr == &attr_info) {
   248         return ec_sdo_entry_info(entry, buffer);
   327         return ec_sdo_entry_info(entry, buffer);
   249     }
   328     }
       
   329     else if (attr == &attr_value) {
       
   330         return ec_sdo_entry_read_value(entry, buffer);
       
   331     }
   250 
   332 
   251     return 0;
   333     return 0;
   252 }
   334 }
   253 
   335 
   254 /*****************************************************************************/
   336 /*****************************************************************************/
   255 
   337 
   256 #if 0
   338 /**
   257 int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT slave */
   339    SDO request constructor.
   258                         uint16_t sdo_index, /**< SDO index */
   340 */
   259                         uint8_t sdo_subindex, /**< SDO subindex */
   341 
   260                         uint8_t *target, /**< memory for value */
   342 void ec_sdo_request_init_read(ec_sdo_request_t *req, /**< SDO request */
   261                         size_t *size /**< target memory size */
   343                               ec_sdo_t *sdo, /**< SDO */
   262                         )
   344                               ec_sdo_entry_t *entry /**< SDO entry */
   263 {
   345                               )
   264     uint8_t *data;
   346 {
   265     size_t rec_size, data_size;
   347     req->sdo = sdo;
   266     uint32_t complete_size;
   348     req->entry = entry;
   267     ec_datagram_t datagram;
   349     req->data = NULL;
   268 
   350     req->size = 0;
   269     ec_datagram_init(&datagram);
   351     req->return_code = 0;
   270 
   352 }
   271     if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6)))
   353 
   272         goto err;
   354 /*****************************************************************************/
   273 
   355 
   274     EC_WRITE_U16(data, 0x2 << 12); // SDO request
   356 /**
   275     EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
   357    SDO request destructor.
   276     EC_WRITE_U16(data + 3, sdo_index);
   358 */
   277     EC_WRITE_U8 (data + 5, sdo_subindex);
   359 
   278 
   360 void ec_sdo_request_clear(ec_sdo_request_t *req /**< SDO request */)
   279     if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size)))
   361 {
   280         goto err;
   362     if (req->data) kfree(req->data);
   281 
   363 }
   282     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
   364 
   283         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
   365 /*****************************************************************************/
   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 /*****************************************************************************/