IgH EtherCAT Master: datagram.c Source File

00001 /******************************************************************************
00002  *
00003  *  $Id$
00004  *
00005  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
00006  *
00007  *  This file is part of the IgH EtherCAT Master.
00008  *
00009  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
00010  *  modify it under the terms of the GNU General Public License version 2, as
00011  *  published by the Free Software Foundation.
00012  *
00013  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
00016  *  Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License along
00019  *  with the IgH EtherCAT Master; if not, write to the Free Software
00020  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  *  ---
00023  *
00024  *  The license mentioned above concerns the source code only. Using the
00025  *  EtherCAT technology and brand is only permitted in compliance with the
00026  *  industrial property and similar rights of Beckhoff Automation GmbH.
00027  *
00028  *****************************************************************************/
00029 
00035 /*****************************************************************************/
00036 
00037 #include <linux/slab.h>
00038 
00039 #include "datagram.h"
00040 #include "master.h"
00041 
00042 /*****************************************************************************/
00043 
00046 #define EC_FUNC_HEADER \
00047     ret = ec_datagram_prealloc(datagram, data_size); \
00048     if (unlikely(ret)) \
00049         return ret; \
00050     datagram->index = 0; \
00051     datagram->working_counter = 0; \
00052     datagram->state = EC_DATAGRAM_INIT;
00053 
00054 #define EC_FUNC_FOOTER \
00055     datagram->data_size = data_size; \
00056     return 0;
00057 
00060 /*****************************************************************************/
00061 
00066 static const char *type_strings[] = {
00067     "?",
00068     "APRD",
00069     "APWR",
00070     "APRW",
00071     "FPRD",
00072     "FPWR",
00073     "FPRW",
00074     "BRD",
00075     "BWR",
00076     "BRW",
00077     "LRD",
00078     "LWR",
00079     "LRW",
00080     "ARMW",
00081     "FRMW"
00082 };
00083 
00084 /*****************************************************************************/
00085 
00088 void ec_datagram_init(ec_datagram_t *datagram )
00089 {
00090     INIT_LIST_HEAD(&datagram->queue); // mark as unqueued
00091     datagram->device_index = EC_DEVICE_MAIN;
00092     datagram->type = EC_DATAGRAM_NONE;
00093     memset(datagram->address, 0x00, EC_ADDR_LEN);
00094     datagram->data = NULL;
00095     datagram->data_origin = EC_ORIG_INTERNAL;
00096     datagram->mem_size = 0;
00097     datagram->data_size = 0;
00098     datagram->index = 0x00;
00099     datagram->working_counter = 0x0000;
00100     datagram->state = EC_DATAGRAM_INIT;
00101 #ifdef EC_HAVE_CYCLES
00102     datagram->cycles_sent = 0;
00103 #endif
00104     datagram->jiffies_sent = 0;
00105 #ifdef EC_HAVE_CYCLES
00106     datagram->cycles_received = 0;
00107 #endif
00108     datagram->jiffies_received = 0;
00109     datagram->skip_count = 0;
00110     datagram->stats_output_jiffies = 0;
00111     memset(datagram->name, 0x00, EC_DATAGRAM_NAME_SIZE);
00112 }
00113 
00114 /*****************************************************************************/
00115 
00118 void ec_datagram_clear(ec_datagram_t *datagram )
00119 {
00120     ec_datagram_unqueue(datagram);
00121 
00122     if (datagram->data_origin == EC_ORIG_INTERNAL && datagram->data) {
00123         kfree(datagram->data);
00124         datagram->data = NULL;
00125     }
00126 }
00127 
00128 /*****************************************************************************/
00129 
00132 void ec_datagram_unqueue(ec_datagram_t *datagram )
00133 {
00134     if (!list_empty(&datagram->queue)) {
00135         list_del_init(&datagram->queue);
00136     }
00137 }
00138 
00139 /*****************************************************************************/
00140 
00150 int ec_datagram_prealloc(
00151         ec_datagram_t *datagram, 
00152         size_t size 
00153         )
00154 {
00155     if (datagram->data_origin == EC_ORIG_EXTERNAL
00156             || size <= datagram->mem_size)
00157         return 0;
00158 
00159     if (datagram->data) {
00160         kfree(datagram->data);
00161         datagram->data = NULL;
00162         datagram->mem_size = 0;
00163     }
00164 
00165     if (!(datagram->data = kmalloc(size, GFP_KERNEL))) {
00166         EC_ERR("Failed to allocate %zu bytes of datagram memory!\n", size);
00167         return -ENOMEM;
00168     }
00169 
00170     datagram->mem_size = size;
00171     return 0;
00172 }
00173 
00174 /*****************************************************************************/
00175 
00178 void ec_datagram_zero(ec_datagram_t *datagram )
00179 {
00180     memset(datagram->data, 0x00, datagram->data_size);
00181 }
00182 
00183 /*****************************************************************************/
00184 
00189 int ec_datagram_aprd(
00190         ec_datagram_t *datagram, 
00191         uint16_t ring_position, 
00192         uint16_t mem_address, 
00193         size_t data_size 
00194         )
00195 {
00196     int ret;
00197     EC_FUNC_HEADER;
00198     datagram->type = EC_DATAGRAM_APRD;
00199     EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));
00200     EC_WRITE_U16(datagram->address + 2, mem_address);
00201     EC_FUNC_FOOTER;
00202 }
00203 
00204 /*****************************************************************************/
00205 
00210 int ec_datagram_apwr(
00211         ec_datagram_t *datagram, 
00212         uint16_t ring_position, 
00213         uint16_t mem_address, 
00214         size_t data_size 
00215         )
00216 {
00217     int ret;
00218     EC_FUNC_HEADER;
00219     datagram->type = EC_DATAGRAM_APWR;
00220     EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));
00221     EC_WRITE_U16(datagram->address + 2, mem_address);
00222     EC_FUNC_FOOTER;
00223 }
00224 
00225 /*****************************************************************************/
00226 
00231 int ec_datagram_aprw(
00232         ec_datagram_t *datagram, 
00233         uint16_t ring_position, 
00234         uint16_t mem_address, 
00235         size_t data_size 
00236         )
00237 {
00238     int ret;
00239     EC_FUNC_HEADER;
00240     datagram->type = EC_DATAGRAM_APRW;
00241     EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));
00242     EC_WRITE_U16(datagram->address + 2, mem_address);
00243     EC_FUNC_FOOTER;
00244 }
00245 
00246 /*****************************************************************************/
00247 
00252 int ec_datagram_armw(
00253         ec_datagram_t *datagram, 
00254         uint16_t ring_position, 
00255         uint16_t mem_address, 
00256         size_t data_size 
00257         )
00258 {
00259     int ret;
00260     EC_FUNC_HEADER;
00261     datagram->type = EC_DATAGRAM_ARMW;
00262     EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));
00263     EC_WRITE_U16(datagram->address + 2, mem_address);
00264     EC_FUNC_FOOTER;
00265 }
00266 
00267 /*****************************************************************************/
00268 
00273 int ec_datagram_fprd(
00274         ec_datagram_t *datagram, 
00275         uint16_t configured_address, 
00276         uint16_t mem_address, 
00277         size_t data_size 
00278         )
00279 {
00280     int ret;
00281 
00282     if (unlikely(configured_address == 0x0000))
00283         EC_WARN("Using configured station address 0x0000!\n");
00284 
00285     EC_FUNC_HEADER;
00286     datagram->type = EC_DATAGRAM_FPRD;
00287     EC_WRITE_U16(datagram->address, configured_address);
00288     EC_WRITE_U16(datagram->address + 2, mem_address);
00289     EC_FUNC_FOOTER;
00290 }
00291 
00292 /*****************************************************************************/
00293 
00298 int ec_datagram_fpwr(
00299         ec_datagram_t *datagram, 
00300         uint16_t configured_address, 
00301         uint16_t mem_address, 
00302         size_t data_size 
00303         )
00304 {
00305     int ret;
00306 
00307     if (unlikely(configured_address == 0x0000))
00308         EC_WARN("Using configured station address 0x0000!\n");
00309 
00310     EC_FUNC_HEADER;
00311     datagram->type = EC_DATAGRAM_FPWR;
00312     EC_WRITE_U16(datagram->address, configured_address);
00313     EC_WRITE_U16(datagram->address + 2, mem_address);
00314     EC_FUNC_FOOTER;
00315 }
00316 
00317 /*****************************************************************************/
00318 
00323 int ec_datagram_fprw(
00324         ec_datagram_t *datagram, 
00325         uint16_t configured_address, 
00326         uint16_t mem_address, 
00327         size_t data_size 
00328         )
00329 {
00330     int ret;
00331 
00332     if (unlikely(configured_address == 0x0000))
00333         EC_WARN("Using configured station address 0x0000!\n");
00334 
00335     EC_FUNC_HEADER;
00336     datagram->type = EC_DATAGRAM_FPRW;
00337     EC_WRITE_U16(datagram->address, configured_address);
00338     EC_WRITE_U16(datagram->address + 2, mem_address);
00339     EC_FUNC_FOOTER;
00340 }
00341 
00342 /*****************************************************************************/
00343 
00348 int ec_datagram_frmw(
00349         ec_datagram_t *datagram, 
00350         uint16_t configured_address, 
00351         uint16_t mem_address, 
00352         size_t data_size 
00353         )
00354 {
00355     int ret;
00356 
00357     if (unlikely(configured_address == 0x0000))
00358         EC_WARN("Using configured station address 0x0000!\n");
00359 
00360     EC_FUNC_HEADER;
00361     datagram->type = EC_DATAGRAM_FRMW;
00362     EC_WRITE_U16(datagram->address, configured_address);
00363     EC_WRITE_U16(datagram->address + 2, mem_address);
00364     EC_FUNC_FOOTER;
00365 }
00366 
00367 /*****************************************************************************/
00368 
00373 int ec_datagram_brd(
00374         ec_datagram_t *datagram, 
00375         uint16_t mem_address, 
00376         size_t data_size 
00377         )
00378 {
00379     int ret;
00380     EC_FUNC_HEADER;
00381     datagram->type = EC_DATAGRAM_BRD;
00382     EC_WRITE_U16(datagram->address, 0x0000);
00383     EC_WRITE_U16(datagram->address + 2, mem_address);
00384     EC_FUNC_FOOTER;
00385 }
00386 
00387 /*****************************************************************************/
00388 
00393 int ec_datagram_bwr(
00394         ec_datagram_t *datagram, 
00395         uint16_t mem_address, 
00396         size_t data_size 
00397         )
00398 {
00399     int ret;
00400     EC_FUNC_HEADER;
00401     datagram->type = EC_DATAGRAM_BWR;
00402     EC_WRITE_U16(datagram->address, 0x0000);
00403     EC_WRITE_U16(datagram->address + 2, mem_address);
00404     EC_FUNC_FOOTER;
00405 }
00406 
00407 /*****************************************************************************/
00408 
00413 int ec_datagram_brw(
00414         ec_datagram_t *datagram, 
00415         uint16_t mem_address, 
00416         size_t data_size 
00417         )
00418 {
00419     int ret;
00420     EC_FUNC_HEADER;
00421     datagram->type = EC_DATAGRAM_BRW;
00422     EC_WRITE_U16(datagram->address, 0x0000);
00423     EC_WRITE_U16(datagram->address + 2, mem_address);
00424     EC_FUNC_FOOTER;
00425 }
00426 
00427 /*****************************************************************************/
00428 
00433 int ec_datagram_lrd(
00434         ec_datagram_t *datagram, 
00435         uint32_t offset, 
00436         size_t data_size 
00437         )
00438 {
00439     int ret;
00440     EC_FUNC_HEADER;
00441     datagram->type = EC_DATAGRAM_LRD;
00442     EC_WRITE_U32(datagram->address, offset);
00443     EC_FUNC_FOOTER;
00444 }
00445 
00446 /*****************************************************************************/
00447 
00452 int ec_datagram_lwr(
00453         ec_datagram_t *datagram, 
00454         uint32_t offset, 
00455         size_t data_size 
00456         )
00457 {
00458     int ret;
00459     EC_FUNC_HEADER;
00460     datagram->type = EC_DATAGRAM_LWR;
00461     EC_WRITE_U32(datagram->address, offset);
00462     EC_FUNC_FOOTER;
00463 }
00464 
00465 /*****************************************************************************/
00466 
00471 int ec_datagram_lrw(
00472         ec_datagram_t *datagram, 
00473         uint32_t offset, 
00474         size_t data_size 
00475         )
00476 {
00477     int ret;
00478     EC_FUNC_HEADER;
00479     datagram->type = EC_DATAGRAM_LRW;
00480     EC_WRITE_U32(datagram->address, offset);
00481     EC_FUNC_FOOTER;
00482 }
00483 
00484 /*****************************************************************************/
00485 
00493 int ec_datagram_lrd_ext(
00494         ec_datagram_t *datagram, 
00495         uint32_t offset, 
00496         size_t data_size, 
00497         uint8_t *external_memory 
00498         )
00499 {
00500     int ret;
00501     datagram->data = external_memory;
00502     datagram->data_origin = EC_ORIG_EXTERNAL;
00503     EC_FUNC_HEADER;
00504     datagram->type = EC_DATAGRAM_LRD;
00505     EC_WRITE_U32(datagram->address, offset);
00506     EC_FUNC_FOOTER;
00507 }
00508 
00509 /*****************************************************************************/
00510 
00518 int ec_datagram_lwr_ext(
00519         ec_datagram_t *datagram, 
00520         uint32_t offset, 
00521         size_t data_size, 
00522         uint8_t *external_memory 
00523         )
00524 {
00525     int ret;
00526     datagram->data = external_memory;
00527     datagram->data_origin = EC_ORIG_EXTERNAL;
00528     EC_FUNC_HEADER;
00529     datagram->type = EC_DATAGRAM_LWR;
00530     EC_WRITE_U32(datagram->address, offset);
00531     EC_FUNC_FOOTER;
00532 }
00533 
00534 /*****************************************************************************/
00535 
00543 int ec_datagram_lrw_ext(
00544         ec_datagram_t *datagram, 
00545         uint32_t offset, 
00546         size_t data_size, 
00547         uint8_t *external_memory 
00548         )
00549 {
00550     int ret;
00551     datagram->data = external_memory;
00552     datagram->data_origin = EC_ORIG_EXTERNAL;
00553     EC_FUNC_HEADER;
00554     datagram->type = EC_DATAGRAM_LRW;
00555     EC_WRITE_U32(datagram->address, offset);
00556     EC_FUNC_FOOTER;
00557 }
00558 
00559 /*****************************************************************************/
00560 
00565 void ec_datagram_print_state(
00566         const ec_datagram_t *datagram 
00567         )
00568 {
00569     printk("Datagram ");
00570     switch (datagram->state) {
00571         case EC_DATAGRAM_INIT:
00572             printk("initialized");
00573             break;
00574         case EC_DATAGRAM_QUEUED:
00575             printk("queued");
00576             break;
00577         case EC_DATAGRAM_SENT:
00578             printk("sent");
00579             break;
00580         case EC_DATAGRAM_RECEIVED:
00581             printk("received");
00582             break;
00583         case EC_DATAGRAM_TIMED_OUT:
00584             printk("timed out");
00585             break;
00586         case EC_DATAGRAM_ERROR:
00587             printk("error");
00588             break;
00589         default:
00590             printk("???");
00591     }
00592 
00593     printk(".\n");
00594 }
00595 
00596 /*****************************************************************************/
00597 
00602 void ec_datagram_print_wc_error(
00603         const ec_datagram_t *datagram 
00604         )
00605 {
00606     if (datagram->working_counter == 0)
00607         printk("No response.");
00608     else if (datagram->working_counter > 1)
00609         printk("%u slaves responded!", datagram->working_counter);
00610     else
00611         printk("Success.");
00612     printk("\n");
00613 }
00614 
00615 /*****************************************************************************/
00616 
00619 void ec_datagram_output_stats(
00620         ec_datagram_t *datagram
00621         )
00622 {
00623     if (jiffies - datagram->stats_output_jiffies > HZ) {
00624         datagram->stats_output_jiffies = jiffies;
00625 
00626         if (unlikely(datagram->skip_count)) {
00627             EC_WARN("Datagram %p (%s) was SKIPPED %u time%s.\n",
00628                     datagram, datagram->name,
00629                     datagram->skip_count,
00630                     datagram->skip_count == 1 ? "" : "s");
00631             datagram->skip_count = 0;
00632         }
00633     }
00634 }
00635 
00636 /*****************************************************************************/
00637 
00642 const char *ec_datagram_type_string(
00643         const ec_datagram_t *datagram 
00644         )
00645 {
00646     return type_strings[datagram->type];
00647 }
00648 
00649 /*****************************************************************************/