Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/canopen.c
branchstable-1.0
changeset 1624 9dc190591c0f
parent 1623 05622513f627
equal deleted inserted replaced
1623:05622513f627 1624:9dc190591c0f 
    46 #include "mailbox.h"
 
    46 #include "mailbox.h"
 
    47 
    47 
    48 /*****************************************************************************/
 
    48 /*****************************************************************************/
 
    49 
    49 
    50 void ec_canopen_abort_msg(uint32_t);
 
    50 void ec_canopen_abort_msg(uint32_t);
 
    51 int ec_slave_fetch_sdo_descriptions(ec_slave_t *, ec_command_t *);
 
    51 int ec_slave_fetch_sdo_descriptions(ec_slave_t *, ec_datagram_t *);
 
    52 int ec_slave_fetch_sdo_entries(ec_slave_t *, ec_command_t *,
 
    52 int ec_slave_fetch_sdo_entries(ec_slave_t *, ec_datagram_t *,
 
    53                                ec_sdo_t *, uint8_t);
 
    53                                ec_sdo_t *, uint8_t);
 
    54 
    54 
    55 /*****************************************************************************/
 
    55 /*****************************************************************************/
 
    56 
    56 
    57 /**
 
    57 /**
 
    63                           uint16_t sdo_index, /**< SDO index */
 
    63                           uint16_t sdo_index, /**< SDO index */
 
    64                           uint8_t sdo_subindex, /**< SDO subindex */
 
    64                           uint8_t sdo_subindex, /**< SDO subindex */
 
    65                           uint8_t *target /**< 4-byte memory */
 
    65                           uint8_t *target /**< 4-byte memory */
 
    66                           )
 
    66                           )
 
    67 {
 
    67 {
 
    68     ec_command_t command;
 
    68     ec_datagram_t datagram;
 
    69     size_t rec_size;
 
    69     size_t rec_size;
 
    70     uint8_t *data;
 
    70     uint8_t *data;
 
    71 
    71 
    72     ec_command_init(&command);
 
    72     ec_datagram_init(&datagram);
 
    73 
    73 
    74     if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 6)))
 
    74     if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6)))
 
    75         goto err;
 
    75         goto err;
 
    76 
    76 
    77     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
    77     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
    78     EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer
 
    78     EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer
 
    79                             | 0x2 << 5)); // initiate upload request
 
    79                             | 0x2 << 5)); // initiate upload request
 
    80     EC_WRITE_U16(data + 3, sdo_index);
 
    80     EC_WRITE_U16(data + 3, sdo_index);
 
    81     EC_WRITE_U8 (data + 5, sdo_subindex);
 
    81     EC_WRITE_U8 (data + 5, sdo_subindex);
 
    82 
    82 
    83     if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size)))
 
    83     if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size)))
 
    84         goto err;
 
    84         goto err;
 
    85 
    85 
    86     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
    86     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
    87         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
    87         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
    88         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
 
    88         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
 
   102         goto err;
 
   102         goto err;
 
   103     }
 
   103     }
 
   104 
   104 
   105     memcpy(target, data + 6, 4);
 
   105     memcpy(target, data + 6, 4);
 
   106 
   106 
   107     ec_command_clear(&command);
 
   107     ec_datagram_clear(&datagram);
 
   108     return 0;
 
   108     return 0;
 
   109  err:
 
   109  err:
 
   110     ec_command_clear(&command);
 
   110     ec_datagram_clear(&datagram);
 
   111     return -1;
 
   111     return -1;
 
   112 }
 
   112 }
 
   113 
   113 
   114 /*****************************************************************************/
 
   114 /*****************************************************************************/
 
   115 
   115 
   125                            size_t size /**< Data size in bytes (1 - 4) */
 
   125                            size_t size /**< Data size in bytes (1 - 4) */
 
   126                            )
 
   126                            )
 
   127 {
 
   127 {
 
   128     uint8_t *data;
 
   128     uint8_t *data;
 
   129     size_t rec_size;
 
   129     size_t rec_size;
 
   130     ec_command_t command;
 
   130     ec_datagram_t datagram;
 
   131 
   131 
   132     ec_command_init(&command);
 
   132     ec_datagram_init(&datagram);
 
   133 
   133 
   134     if (size == 0 || size > 4) {
 
   134     if (size == 0 || size > 4) {
 
   135         EC_ERR("Invalid data size!\n");
 
   135         EC_ERR("Invalid data size!\n");
 
   136         goto err;
 
   136         goto err;
 
   137     }
 
   137     }
 
   138 
   138 
   139     if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 0x0A)))
 
   139     if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 0x0A)))
 
   140         goto err;
 
   140         goto err;
 
   141 
   141 
   142     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
   142     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
   143     EC_WRITE_U8 (data + 2, (0x1 // size specified
 
   143     EC_WRITE_U8 (data + 2, (0x1 // size specified
 
   144                             | 0x1 << 1 // expedited transfer
 
   144                             | 0x1 << 1 // expedited transfer
 
   147     EC_WRITE_U16(data + 3, sdo_index);
 
   147     EC_WRITE_U16(data + 3, sdo_index);
 
   148     EC_WRITE_U8 (data + 5, sdo_subindex);
 
   148     EC_WRITE_U8 (data + 5, sdo_subindex);
 
   149     memcpy(data + 6, sdo_data, size);
 
   149     memcpy(data + 6, sdo_data, size);
 
   150     if (size < 4) memset(data + 6 + size, 0x00, 4 - size);
 
   150     if (size < 4) memset(data + 6 + size, 0x00, 4 - size);
 
   151 
   151 
   152     if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size)))
 
   152     if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size)))
 
   153         goto err;
 
   153         goto err;
 
   154 
   154 
   155     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
   155     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
   156         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
   156         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
   157         EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n",
 
   157         EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n",
 
   170                slave->ring_position);
 
   170                slave->ring_position);
 
   171         ec_print_data(data, rec_size);
 
   171         ec_print_data(data, rec_size);
 
   172         return -1;
 
   172         return -1;
 
   173     }
 
   173     }
 
   174 
   174 
   175     ec_command_clear(&command);
 
   175     ec_datagram_clear(&datagram);
 
   176     return 0;
 
   176     return 0;
 
   177  err:
 
   177  err:
 
   178     ec_command_clear(&command);
 
   178     ec_datagram_clear(&datagram);
 
   179     return -1;
 
   179     return -1;
 
   180 }
 
   180 }
 
   181 
   181 
   182 /*****************************************************************************/
 
   182 /*****************************************************************************/
 
   183 
   183 
   196                         )
 
   196                         )
 
   197 {
 
   197 {
 
   198     uint8_t *data;
 
   198     uint8_t *data;
 
   199     size_t rec_size, data_size;
 
   199     size_t rec_size, data_size;
 
   200     uint32_t complete_size;
 
   200     uint32_t complete_size;
 
   201     ec_command_t command;
 
   201     ec_datagram_t datagram;
 
   202 
   202 
   203     ec_command_init(&command);
 
   203     ec_datagram_init(&datagram);
 
   204 
   204 
   205     if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 6)))
 
   205     if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6)))
 
   206         goto err;
 
   206         goto err;
 
   207 
   207 
   208     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
   208     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
   209     EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
 
   209     EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
 
   210     EC_WRITE_U16(data + 3, sdo_index);
 
   210     EC_WRITE_U16(data + 3, sdo_index);
 
   211     EC_WRITE_U8 (data + 5, sdo_subindex);
 
   211     EC_WRITE_U8 (data + 5, sdo_subindex);
 
   212 
   212 
   213     if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size)))
 
   213     if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size)))
 
   214         goto err;
 
   214         goto err;
 
   215 
   215 
   216     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
   216     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
   217         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
   217         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
   218         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
 
   218         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
 
   251         goto err;
 
   251         goto err;
 
   252     }
 
   252     }
 
   253 
   253 
   254     memcpy(target, data + 10, data_size);
 
   254     memcpy(target, data + 10, data_size);
 
   255 
   255 
   256     ec_command_clear(&command);
 
   256     ec_datagram_clear(&datagram);
 
   257     return 0;
 
   257     return 0;
 
   258  err:
 
   258  err:
 
   259     ec_command_clear(&command);
 
   259     ec_datagram_clear(&datagram);
 
   260     return -1;
 
   260     return -1;
 
   261 }
 
   261 }
 
   262 
   262 
   263 /*****************************************************************************/
 
   263 /*****************************************************************************/
 
   264 
   264 
   272     uint8_t *data;
 
   272     uint8_t *data;
 
   273     size_t rec_size;
 
   273     size_t rec_size;
 
   274     unsigned int i, sdo_count;
 
   274     unsigned int i, sdo_count;
 
   275     ec_sdo_t *sdo;
 
   275     ec_sdo_t *sdo;
 
   276     uint16_t sdo_index;
 
   276     uint16_t sdo_index;
 
   277     ec_command_t command;
 
   277     ec_datagram_t datagram;
 
   278 
   278 
   279     ec_command_init(&command);
 
   279     ec_datagram_init(&datagram);
 
   280 
   280 
   281     if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 8)))
 
   281     if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 8)))
 
   282         goto err;
 
   282         goto err;
 
   283 
   283 
   284     EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
   284     EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
   285     EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request
 
   285     EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request
 
   286     EC_WRITE_U8 (data + 3, 0x00);
 
   286     EC_WRITE_U8 (data + 3, 0x00);
 
   287     EC_WRITE_U16(data + 4, 0x0000);
 
   287     EC_WRITE_U16(data + 4, 0x0000);
 
   288     EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!
 
   288     EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!
 
   289 
   289 
   290     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   290     if (unlikely(ec_master_simple_io(slave->master, &datagram))) {
 
   291         EC_ERR("Mailbox checking failed on slave %i!\n", slave->ring_position);
 
   291         EC_ERR("Mailbox checking failed on slave %i!\n", slave->ring_position);
 
   292         goto err;
 
   292         goto err;
 
   293     }
 
   293     }
 
   294 
   294 
   295     do {
 
   295     do {
 
   296         if (!(data = ec_slave_mbox_simple_receive(slave, &command,
 
   296         if (!(data = ec_slave_mbox_simple_receive(slave, &datagram,
 
   297                                                   0x03, &rec_size)))
 
   297                                                   0x03, &rec_size)))
 
   298             goto err;
 
   298             goto err;
 
   299 
   299 
   300         if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   300         if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   301             (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
 
   301             (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
 
   340         }
 
   340         }
 
   341     }
 
   341     }
 
   342     while (EC_READ_U8(data + 2) & 0x80);
 
   342     while (EC_READ_U8(data + 2) & 0x80);
 
   343 
   343 
   344     // Fetch all SDO descriptions
 
   344     // Fetch all SDO descriptions
 
   345     if (ec_slave_fetch_sdo_descriptions(slave, &command)) goto err;
 
   345     if (ec_slave_fetch_sdo_descriptions(slave, &datagram)) goto err;
 
   346 
   346 
   347     ec_command_clear(&command);
 
   347     ec_datagram_clear(&datagram);
 
   348     return 0;
 
   348     return 0;
 
   349  err:
 
   349  err:
 
   350     ec_command_clear(&command);
 
   350     ec_datagram_clear(&datagram);
 
   351     return -1;
 
   351     return -1;
 
   352 }
 
   352 }
 
   353 
   353 
   354 /*****************************************************************************/
 
   354 /*****************************************************************************/
 
   355 
   355 
   357    Fetches the SDO descriptions for the known SDOs.
 
   357    Fetches the SDO descriptions for the known SDOs.
 
   358    \return 0 in case of success, else < 0
 
   358    \return 0 in case of success, else < 0
 
   359 */
 
   359 */
 
   360 
   360 
   361 int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave, /**< EtherCAT slave */
 
   361 int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave, /**< EtherCAT slave */
 
   362                                     ec_command_t *command /**< command */
 
   362                                     ec_datagram_t *datagram /**< datagram */
 
   363                                     )
 
   363                                     )
 
   364 {
 
   364 {
 
   365     uint8_t *data;
 
   365     uint8_t *data;
 
   366     size_t rec_size, name_size;
 
   366     size_t rec_size, name_size;
 
   367     ec_sdo_t *sdo;
 
   367     ec_sdo_t *sdo;
 
   368 
   368 
   369     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
 
   369     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
 
   370         if (!(data = ec_slave_mbox_prepare_send(slave, command, 0x03, 8)))
 
   370         if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8)))
 
   371             return -1;
 
   371             return -1;
 
   372         EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
   372         EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
   373         EC_WRITE_U8 (data + 2, 0x03); // Get object description request
 
   373         EC_WRITE_U8 (data + 2, 0x03); // Get object description request
 
   374         EC_WRITE_U8 (data + 3, 0x00);
 
   374         EC_WRITE_U8 (data + 3, 0x00);
 
   375         EC_WRITE_U16(data + 4, 0x0000);
 
   375         EC_WRITE_U16(data + 4, 0x0000);
 
   376         EC_WRITE_U16(data + 6, sdo->index); // SDO index
 
   376         EC_WRITE_U16(data + 6, sdo->index); // SDO index
 
   377 
   377 
   378         if (!(data = ec_slave_mbox_simple_io(slave, command, &rec_size)))
 
   378         if (!(data = ec_slave_mbox_simple_io(slave, datagram, &rec_size)))
 
   379             return -1;
 
   379             return -1;
 
   380 
   380 
   381         if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   381         if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   382             (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
 
   382             (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
 
   383             EC_ERR("SDO information error response at slave %i while"
 
   383             EC_ERR("SDO information error response at slave %i while"
 
   426             EC_ERR("Fragment follows (not implemented)!\n");
 
   426             EC_ERR("Fragment follows (not implemented)!\n");
 
   427             return -1;
 
   427             return -1;
 
   428         }
 
   428         }
 
   429 
   429 
   430         // Fetch all entries (subindices)
 
   430         // Fetch all entries (subindices)
 
   431         if (ec_slave_fetch_sdo_entries(slave, command, sdo,
 
   431         if (ec_slave_fetch_sdo_entries(slave, datagram, sdo,
 
   432                                        EC_READ_U8(data + 10)))
 
   432                                        EC_READ_U8(data + 10)))
 
   433             return -1;
 
   433             return -1;
 
   434     }
 
   434     }
 
   435 
   435 
   436     return 0;
 
   436     return 0;
 
   442    Fetches all entries (subindices) to an SDO.
 
   442    Fetches all entries (subindices) to an SDO.
 
   443    \return 0 in case of success, else < 0
 
   443    \return 0 in case of success, else < 0
 
   444 */
 
   444 */
 
   445 
   445 
   446 int ec_slave_fetch_sdo_entries(ec_slave_t *slave, /**< EtherCAT slave */
 
   446 int ec_slave_fetch_sdo_entries(ec_slave_t *slave, /**< EtherCAT slave */
 
   447                                ec_command_t *command, /**< command */
 
   447                                ec_datagram_t *datagram, /**< datagram */
 
   448                                ec_sdo_t *sdo, /**< SDO */
 
   448                                ec_sdo_t *sdo, /**< SDO */
 
   449                                uint8_t subindices /**< number of subindices */
 
   449                                uint8_t subindices /**< number of subindices */
 
   450                                )
 
   450                                )
 
   451 {
 
   451 {
 
   452     uint8_t *data;
 
   452     uint8_t *data;
 
   453     size_t rec_size, data_size;
 
   453     size_t rec_size, data_size;
 
   454     uint8_t i;
 
   454     uint8_t i;
 
   455     ec_sdo_entry_t *entry;
 
   455     ec_sdo_entry_t *entry;
 
   456 
   456 
   457     for (i = 1; i <= subindices; i++) {
 
   457     for (i = 1; i <= subindices; i++) {
 
   458         if (!(data = ec_slave_mbox_prepare_send(slave, command, 0x03, 10)))
 
   458         if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10)))
 
   459             return -1;
 
   459             return -1;
 
   460 
   460 
   461         EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
   461         EC_WRITE_U16(data, 0x8 << 12); // SDO information
 
   462         EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
 
   462         EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
 
   463         EC_WRITE_U8 (data + 3, 0x00);
 
   463         EC_WRITE_U8 (data + 3, 0x00);
 
   464         EC_WRITE_U16(data + 4, 0x0000);
 
   464         EC_WRITE_U16(data + 4, 0x0000);
 
   465         EC_WRITE_U16(data + 6, sdo->index); // SDO index
 
   465         EC_WRITE_U16(data + 6, sdo->index); // SDO index
 
   466         EC_WRITE_U8 (data + 8, i); // SDO subindex
 
   466         EC_WRITE_U8 (data + 8, i); // SDO subindex
 
   467         EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
 
   467         EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
 
   468 
   468 
   469         if (!(data = ec_slave_mbox_simple_io(slave, command, &rec_size)))
 
   469         if (!(data = ec_slave_mbox_simple_io(slave, datagram, &rec_size)))
 
   470             return -1;
 
   470             return -1;
 
   471 
   471 
   472         if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   472         if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   473             (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
 
   473             (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
 
   474             EC_ERR("SDO information error response at slave %i while"
 
   474             EC_ERR("SDO information error response at slave %i while"
etherlabmaster