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

   Methods of an EtherCAT datagram.

    37

*/

    38

    39

/*****************************************************************************/

    40

    41

#include <linux/slab.h>

    42

    43

#include "datagram.h"

    44

#include "master.h"

    45

    46

/*****************************************************************************/

    47

    48

/** \cond */

    49

    50

#define EC_FUNC_HEADER \

    51

    if (unlikely(ec_datagram_prealloc(datagram, data_size))) \

    52

        return -1; \

    53

    datagram->index = 0; \

    54

    datagram->working_counter = 0; \

    55

    datagram->state = EC_DATAGRAM_INIT;

    56

    57

#define EC_FUNC_FOOTER \

    58

    datagram->data_size = data_size; \

    59

    memset(datagram->data, 0x00, data_size); \

    60

    return 0;

    61

    62

/** \endcond */

    63

    64

/*****************************************************************************/

    65

    66

/**

    67

   Datagram constructor.

    68

*/

    69

    70

void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram */)

    71

{

    72

    INIT_LIST_HEAD(&datagram->queue); // mark as unqueued

    73

    datagram->type = EC_DATAGRAM_NONE;

    74

    memset(datagram->address, 0x00, EC_ADDR_LEN);

    75

    datagram->data = NULL;

    76

    datagram->mem_size = 0;

    77

    datagram->data_size = 0;

    78

    datagram->index = 0x00;

    79

    datagram->working_counter = 0x00;

    80

    datagram->state = EC_DATAGRAM_INIT;

    81

    datagram->cycles_sent = 0;

    82

    datagram->jiffies_sent = 0;

    83

    datagram->cycles_received = 0;

    84

    datagram->jiffies_received = 0;

    85

    datagram->skip_count = 0;

    86

    datagram->stats_output_jiffies = 0;

    87

    datagram->name[0] = 0x00;

    88

}

    89

    90

/*****************************************************************************/

    91

    92

/**

    93

   Datagram destructor.

    94

*/

    95

    96

void ec_datagram_clear(ec_datagram_t *datagram /**< EtherCAT datagram */)

    97

{

    98

    if (datagram->data) kfree(datagram->data);

    99

}

   100

   101

/*****************************************************************************/

   102

   103

/**

   104

   Allocates datagram data memory.

   105

   If the allocated memory is already larger than requested, nothing ist done.

   106

   \return 0 in case of success, else < 0

   107

*/

   108

   109

int ec_datagram_prealloc(ec_datagram_t *datagram, /**< EtherCAT datagram */

   110

                         size_t size /**< New size in bytes */

   111

                         )

   112

{

   113

    if (size <= datagram->mem_size) return 0;

   114

   115

    if (datagram->data) {

   116

        kfree(datagram->data);

   117

        datagram->data = NULL;

   118

        datagram->mem_size = 0;

   119

    }

   120

   121

    if (!(datagram->data = kmalloc(size, GFP_ATOMIC))) {

   122

        EC_ERR("Failed to allocate %i bytes of datagram memory!\n", size);

   123

        return -1;

   124

    }

   125

   126

    datagram->mem_size = size;

   127

    return 0;

   128

}

   129

   130

/*****************************************************************************/

   131

   132

/**

   133

   Initializes an EtherCAT NPRD datagram.

   134

   Node-adressed physical read.

   135

   \return 0 in case of success, else < 0

   136

*/

   137

   138

int ec_datagram_nprd(ec_datagram_t *datagram,

   139

                     /**< EtherCAT datagram */

   140

                     uint16_t node_address,

   141

                     /**< configured station address */

   142

                     uint16_t offset,

   143

                     /**< physical memory address */

   144

                     size_t data_size

   145

                     /**< number of bytes to read */

   146

                     )

   147

{

   148

    if (unlikely(node_address == 0x0000))

   149

        EC_WARN("Using node address 0x0000!\n");

   150

   151

    EC_FUNC_HEADER;

   152

    datagram->type = EC_DATAGRAM_NPRD;

   153

    EC_WRITE_U16(datagram->address, node_address);

   154

    EC_WRITE_U16(datagram->address + 2, offset);

   155

    EC_FUNC_FOOTER;

   156

}

   157

   158

/*****************************************************************************/

   159

   160

/**

   161

   Initializes an EtherCAT NPWR datagram.

   162

   Node-adressed physical write.

   163

   \return 0 in case of success, else < 0

   164

*/

   165

   166

int ec_datagram_npwr(ec_datagram_t *datagram,

   167

                     /**< EtherCAT datagram */

   168

                     uint16_t node_address,

   169

                     /**< configured station address */

   170

                     uint16_t offset,

   171

                     /**< physical memory address */

   172

                     size_t data_size

   173

                     /**< number of bytes to write */

   174

                     )

   175

{

   176

    if (unlikely(node_address == 0x0000))

   177

        EC_WARN("Using node address 0x0000!\n");

   178

   179

    EC_FUNC_HEADER;

   180

    datagram->type = EC_DATAGRAM_NPWR;

   181

    EC_WRITE_U16(datagram->address, node_address);

   182

    EC_WRITE_U16(datagram->address + 2, offset);

   183

    EC_FUNC_FOOTER;

   184

}

   185

   186

/*****************************************************************************/

   187

   188

/**

   189

   Initializes an EtherCAT APRD datagram.

   190

   Autoincrement physical read.

   191

   \return 0 in case of success, else < 0

   192

*/

   193

   194

int ec_datagram_aprd(ec_datagram_t *datagram,

   195

                     /**< EtherCAT datagram */

   196

                     uint16_t ring_position,

   197

                     /**< auto-increment position */

   198

                     uint16_t offset,

   199

                     /**< physical memory address */

   200

                     size_t data_size

   201

                     /**< number of bytes to read */

   202

                     )

   203

{

   204

    EC_FUNC_HEADER;

   205

    datagram->type = EC_DATAGRAM_APRD;

   206

    EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));

   207

    EC_WRITE_U16(datagram->address + 2, offset);

   208

    EC_FUNC_FOOTER;

   209

}

   210

   211

/*****************************************************************************/

   212

   213

/**

   214

   Initializes an EtherCAT APWR datagram.

   215

   Autoincrement physical write.

   216

   \return 0 in case of success, else < 0

   217

*/

   218

   219

int ec_datagram_apwr(ec_datagram_t *datagram,

   220

                     /**< EtherCAT datagram */

   221

                     uint16_t ring_position,

   222

                     /**< auto-increment position */

   223

                     uint16_t offset,

   224

                     /**< physical memory address */

   225

                     size_t data_size

   226

                     /**< number of bytes to write */

   227

                     )

   228

{

   229

    EC_FUNC_HEADER;

   230

    datagram->type = EC_DATAGRAM_APWR;

   231

    EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));

   232

    EC_WRITE_U16(datagram->address + 2, offset);

   233

    EC_FUNC_FOOTER;

   234

}

   235

   236

/*****************************************************************************/

   237

   238

/**

   239

   Initializes an EtherCAT BRD datagram.

   240

   Broadcast read.

   241

   \return 0 in case of success, else < 0

   242

*/

   243

   244

int ec_datagram_brd(ec_datagram_t *datagram,

   245

                    /**< EtherCAT datagram */

   246

                    uint16_t offset,

   247

                    /**< physical memory address */

   248

                    size_t data_size

   249

                    /**< number of bytes to read */

   250

                    )

   251

{

   252

    EC_FUNC_HEADER;

   253

    datagram->type = EC_DATAGRAM_BRD;

   254

    EC_WRITE_U16(datagram->address, 0x0000);

   255

    EC_WRITE_U16(datagram->address + 2, offset);

   256

    EC_FUNC_FOOTER;

   257

}

   258

   259

/*****************************************************************************/

   260

   261

/**

   262

   Initializes an EtherCAT BWR datagram.

   263

   Broadcast write.

   264

   \return 0 in case of success, else < 0

   265

*/

   266

   267

int ec_datagram_bwr(ec_datagram_t *datagram,

   268

                    /**< EtherCAT datagram */

   269

                    uint16_t offset,

   270

                    /**< physical memory address */

   271

                    size_t data_size

   272

                    /**< number of bytes to write */

   273

                    )

   274

{

   275

    EC_FUNC_HEADER;

   276

    datagram->type = EC_DATAGRAM_BWR;

   277

    EC_WRITE_U16(datagram->address, 0x0000);

   278

    EC_WRITE_U16(datagram->address + 2, offset);

   279

    EC_FUNC_FOOTER;

   280

}

   281

   282

/*****************************************************************************/

   283

   284

/**

   285

   Initializes an EtherCAT LRW datagram.

   286

   Logical read write.

   287

   \return 0 in case of success, else < 0

   288

*/

   289

   290

int ec_datagram_lrw(ec_datagram_t *datagram,

   291

                    /**< EtherCAT datagram */

   292

                    uint32_t offset,

   293

                    /**< logical address */

   294

                    size_t data_size

   295

                    /**< number of bytes to read/write */

   296

                    )

   297

{

   298

    EC_FUNC_HEADER;

   299

    datagram->type = EC_DATAGRAM_LRW;

   300

    EC_WRITE_U32(datagram->address, offset);

   301

    EC_FUNC_FOOTER;

   302

}

   303

   304

/*****************************************************************************/

   305

   306

/**

   307

 * Evaluates the working counter of a single-cast datagram.

   308

 * Outputs an error message.

   309

 */

   310

   311

void ec_datagram_print_wc_error(

   312

        const ec_datagram_t *datagram

   313

        )

   314

{

   315

    if (datagram->working_counter == 0)

   316

        printk("No response.");

   317

    else if (datagram->working_counter > 1)

   318

        printk("%u slaves responded!", datagram->working_counter);

   319

    else

   320

        printk("Success.");

   321

    printk("\n");

   322

}

   323

   324

/*****************************************************************************/

   325

   326

/**

   327

 * Outputs datagram statistics at most every second.

   328

 */

   329

   330

void ec_datagram_output_stats(

   331

        ec_datagram_t *datagram

   332

        )

   333

{

   334

    if (jiffies - datagram->stats_output_jiffies < HZ) {

   335

        datagram->stats_output_jiffies = jiffies;

   336

   337

        if (unlikely(datagram->skip_count)) {

   338

            EC_WARN("Datagram %x (%s) was SKIPPED %u time%s.\n",

   339

                    (unsigned int) datagram, datagram->name,

   340

                    datagram->skip_count,

   341

                    datagram->skip_count == 1 ? "" : "s");

   342

            datagram->skip_count = 0;

   343

        }

   344

    }

   345

}

   346

   347

/*****************************************************************************/