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

    return 0;

    60

    61

/** \endcond */

    62

    63

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

    64

    65

/** Array of datagram type strings used in ec_datagram_type_string().

    66

 *

    67

 * \attention This is indexed by ec_datagram_type_t.

    68

 */

    69

static const char *type_strings[] = {

    70

    "?",

    71

    "APRD",

    72

    "APWR",

    73

    "APRW",

    74

    "FPRD",

    75

    "FPWR",

    76

    "FPRW",

    77

    "BRD",

    78

    "BWR",

    79

    "BRW",

    80

    "LRD",

    81

    "LWR",

    82

    "LRW",

    83

    "ARMW",

    84

    "FRMW"

    85

};

    86

    87

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

    88

    89

/** Constructor.

    90

 */

    91

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

    92

{

    93

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

    94

    datagram->type = EC_DATAGRAM_NONE;

    95

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

    96

    datagram->data = NULL;

    97

    datagram->data_origin = EC_ORIG_INTERNAL;

    98

    datagram->mem_size = 0;

    99

    datagram->data_size = 0;

   100

    datagram->index = 0x00;

   101

    datagram->working_counter = 0x0000;

   102

    datagram->state = EC_DATAGRAM_INIT;

   103

#ifdef EC_HAVE_CYCLES

   104

    datagram->cycles_sent = 0;

   105

#endif

   106

    datagram->jiffies_sent = 0;

   107

#ifdef EC_HAVE_CYCLES

   108

    datagram->cycles_received = 0;

   109

#endif

   110

    datagram->jiffies_received = 0;

   111

    datagram->skip_count = 0;

   112

    datagram->stats_output_jiffies = 0;

   113

    memset(datagram->name, 0x00, EC_DATAGRAM_NAME_SIZE);

   114

}

   115

   116

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

   117

   118

/** Destructor.

   119

 */

   120

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

   121

{

   122

    if (datagram->data_origin == EC_ORIG_INTERNAL && datagram->data)

   123

        kfree(datagram->data);

   124

}

   125

   126

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

   127

   128

/** Allocates internal payload memory.

   129

 *

   130

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

   131

 *

   132

 * \attention If external payload memory has been provided, no range checking

   133

 *            is done!

   134

 *

   135

 * \return 0 in case of success, else < 0

   136

 */

   137

int ec_datagram_prealloc(

   138

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   139

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

   140

        )

   141

{

   142

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   143

            || size <= datagram->mem_size)

   144

        return 0;

   145

   146

    if (datagram->data) {

   147

        kfree(datagram->data);

   148

        datagram->data = NULL;

   149

        datagram->mem_size = 0;

   150

    }

   151

   152

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

   153

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

   154

        return -1;

   155

    }

   156

   157

    datagram->mem_size = size;

   158

    return 0;

   159

}

   160

   161

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

   162

   163

/** Fills the datagram payload memory with zeros.

   164

 */

   165

void ec_datagram_zero(ec_datagram_t *datagram /**< EtherCAT datagram. */)

   166

{

   167

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

   168

}

   169

   170

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

   171

   172

/** Initializes an EtherCAT APRD datagram.

   173

 *

   174

 * \return 0 in case of success, else < 0

   175

 */

   176

int ec_datagram_aprd(

   177

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   178

        uint16_t ring_position, /**< Auto-increment address. */

   179

        uint16_t mem_address, /**< Physical memory address. */

   180

        size_t data_size /**< Number of bytes to read. */

   181

        )

   182

{

   183

    EC_FUNC_HEADER;

   184

    datagram->type = EC_DATAGRAM_APRD;

   185

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

   186

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

   187

    EC_FUNC_FOOTER;

   188

}

   189

   190

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

   191

   192

/** Initializes an EtherCAT APWR datagram.

   193

 *

   194

 * \return 0 in case of success, else < 0

   195

 */

   196

int ec_datagram_apwr(

   197

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   198

        uint16_t ring_position, /**< Auto-increment address. */

   199

        uint16_t mem_address, /**< Physical memory address. */

   200

        size_t data_size /**< Number of bytes to write. */

   201

        )

   202

{

   203

    EC_FUNC_HEADER;

   204

    datagram->type = EC_DATAGRAM_APWR;

   205

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

   206

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

   207

    EC_FUNC_FOOTER;

   208

}

   209

   210

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

   211

   212

/** Initializes an EtherCAT APRW datagram.

   213

 *

   214

 * \return 0 in case of success, else < 0

   215

 */

   216

int ec_datagram_aprw(

   217

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   218

        uint16_t ring_position, /**< Auto-increment address. */

   219

        uint16_t mem_address, /**< Physical memory address. */

   220

        size_t data_size /**< Number of bytes to write. */

   221

        )

   222

{

   223

    EC_FUNC_HEADER;

   224

    datagram->type = EC_DATAGRAM_APRW;

   225

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

   226

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

   227

    EC_FUNC_FOOTER;

   228

}

   229

   230

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

   231

   232

/** Initializes an EtherCAT ARMW datagram.

   233

 *

   234

 * \return 0 in case of success, else < 0

   235

 */

   236

int ec_datagram_armw(

   237

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   238

        uint16_t ring_position, /**< Auto-increment address. */

   239

        uint16_t mem_address, /**< Physical memory address. */

   240

        size_t data_size /**< Number of bytes to read. */

   241

        )

   242

{

   243

    EC_FUNC_HEADER;

   244

    datagram->type = EC_DATAGRAM_ARMW;

   245

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

   246

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

   247

    EC_FUNC_FOOTER;

   248

}

   249

   250

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

   251

   252

/** Initializes an EtherCAT FPRD datagram.

   253

 *

   254

 * \return 0 in case of success, else < 0

   255

 */

   256

int ec_datagram_fprd(

   257

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   258

        uint16_t configured_address, /**< Configured station address. */

   259

        uint16_t mem_address, /**< Physical memory address. */

   260

        size_t data_size /**< Number of bytes to read. */

   261

        )

   262

{

   263

    if (unlikely(configured_address == 0x0000))

   264

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

   265

   266

    EC_FUNC_HEADER;

   267

    datagram->type = EC_DATAGRAM_FPRD;

   268

    EC_WRITE_U16(datagram->address, configured_address);

   269

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

   270

    EC_FUNC_FOOTER;

   271

}

   272

   273

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

   274

   275

/** Initializes an EtherCAT FPWR datagram.

   276

 *

   277

 * \return 0 in case of success, else < 0

   278

 */

   279

int ec_datagram_fpwr(

   280

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   281

        uint16_t configured_address, /**< Configured station address. */

   282

        uint16_t mem_address, /**< Physical memory address. */

   283

        size_t data_size /**< Number of bytes to write. */

   284

        )

   285

{

   286

    if (unlikely(configured_address == 0x0000))

   287

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

   288

   289

    EC_FUNC_HEADER;

   290

    datagram->type = EC_DATAGRAM_FPWR;

   291

    EC_WRITE_U16(datagram->address, configured_address);

   292

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

   293

    EC_FUNC_FOOTER;

   294

}

   295

   296

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

   297

   298

/** Initializes an EtherCAT FPRW datagram.

   299

 *

   300

 * \return 0 in case of success, else < 0

   301

 */

   302

int ec_datagram_fprw(

   303

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   304

        uint16_t configured_address, /**< Configured station address. */

   305

        uint16_t mem_address, /**< Physical memory address. */

   306

        size_t data_size /**< Number of bytes to write. */

   307

        )

   308

{

   309

    if (unlikely(configured_address == 0x0000))

   310

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

   311

   312

    EC_FUNC_HEADER;

   313

    datagram->type = EC_DATAGRAM_FPRW;

   314

    EC_WRITE_U16(datagram->address, configured_address);

   315

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

   316

    EC_FUNC_FOOTER;

   317

}

   318

   319

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

   320

   321

/** Initializes an EtherCAT FRMW datagram.

   322

 *

   323

 * \return 0 in case of success, else < 0

   324

 */

   325

int ec_datagram_frmw(

   326

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   327

        uint16_t configured_address, /**< Configured station address. */

   328

        uint16_t mem_address, /**< Physical memory address. */

   329

        size_t data_size /**< Number of bytes to write. */

   330

        )

   331

{

   332

    if (unlikely(configured_address == 0x0000))

   333

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

   334

   335

    EC_FUNC_HEADER;

   336

    datagram->type = EC_DATAGRAM_FRMW;

   337

    EC_WRITE_U16(datagram->address, configured_address);

   338

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

   339

    EC_FUNC_FOOTER;

   340

}

   341

   342

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

   343

   344

/** Initializes an EtherCAT BRD datagram.

   345

 *

   346

 * \return 0 in case of success, else < 0

   347

 */

   348

int ec_datagram_brd(

   349

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   350

        uint16_t mem_address, /**< Physical memory address. */

   351

        size_t data_size /**< Number of bytes to read. */

   352

        )

   353

{

   354

    EC_FUNC_HEADER;

   355

    datagram->type = EC_DATAGRAM_BRD;

   356

    EC_WRITE_U16(datagram->address, 0x0000);

   357

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

   358

    EC_FUNC_FOOTER;

   359

}

   360

   361

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

   362

   363

/** Initializes an EtherCAT BWR datagram.

   364

 *

   365

 * \return 0 in case of success, else < 0

   366

 */

   367

int ec_datagram_bwr(

   368

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   369

        uint16_t mem_address, /**< Physical memory address. */

   370

        size_t data_size /**< Number of bytes to write. */

   371

        )

   372

{

   373

    EC_FUNC_HEADER;

   374

    datagram->type = EC_DATAGRAM_BWR;

   375

    EC_WRITE_U16(datagram->address, 0x0000);

   376

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

   377

    EC_FUNC_FOOTER;

   378

}

   379

   380

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

   381

   382

/** Initializes an EtherCAT BRW datagram.

   383

 *

   384

 * \return 0 in case of success, else < 0

   385

 */

   386

int ec_datagram_brw(

   387

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   388

        uint16_t mem_address, /**< Physical memory address. */

   389

        size_t data_size /**< Number of bytes to write. */

   390

        )

   391

{

   392

    EC_FUNC_HEADER;

   393

    datagram->type = EC_DATAGRAM_BRW;

   394

    EC_WRITE_U16(datagram->address, 0x0000);

   395

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

   396

    EC_FUNC_FOOTER;

   397

}

   398

   399

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

   400

   401

/** Initializes an EtherCAT LRD datagram.

   402

 *

   403

 * \attention It is assumed, that the external memory is at least \a data_size

   404

 *            bytes large.

   405

 *

   406

 * \return 0 in case of success, else < 0

   407

 */

   408

int ec_datagram_lrd(

   409

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   410

        uint32_t offset, /**< Logical address. */

   411

        size_t data_size, /**< Number of bytes to read/write. */

   412

        uint8_t *external_memory /**< Pointer to the memory to use. */

   413

        )

   414

{

   415

    datagram->data = external_memory;

   416

    datagram->data_origin = EC_ORIG_EXTERNAL;

   417

    EC_FUNC_HEADER;

   418

    datagram->type = EC_DATAGRAM_LRD;

   419

    EC_WRITE_U32(datagram->address, offset);

   420

    EC_FUNC_FOOTER;

   421

}

   422

   423

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

   424

   425

/** Initializes an EtherCAT LWR datagram.

   426

 *

   427

 * \attention It is assumed, that the external memory is at least \a data_size

   428

 *            bytes large.

   429

 *

   430

 * \return 0 in case of success, else < 0

   431

 */

   432

int ec_datagram_lwr(

   433

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   434

        uint32_t offset, /**< Logical address. */

   435

        size_t data_size, /**< Number of bytes to read/write. */

   436

        uint8_t *external_memory /**< Pointer to the memory to use. */

   437

        )

   438

{

   439

    datagram->data = external_memory;

   440

    datagram->data_origin = EC_ORIG_EXTERNAL;

   441

    EC_FUNC_HEADER;

   442

    datagram->type = EC_DATAGRAM_LWR;

   443

    EC_WRITE_U32(datagram->address, offset);

   444

    EC_FUNC_FOOTER;

   445

}

   446

   447

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

   448

   449

/** Initializes an EtherCAT LRW datagram.

   450

 *

   451

 * \attention It is assumed, that the external memory is at least \a data_size

   452

 *            bytes large.

   453

 *

   454

 * \return 0 in case of success, else < 0

   455

 */

   456

int ec_datagram_lrw(

   457

        ec_datagram_t *datagram, /**< EtherCAT datagram. */

   458

        uint32_t offset, /**< Logical address. */

   459

        size_t data_size, /**< Number of bytes to read/write. */

   460

        uint8_t *external_memory /**< Pointer to the memory to use. */

   461

        )

   462

{

   463

    datagram->data = external_memory;

   464

    datagram->data_origin = EC_ORIG_EXTERNAL;

   465

    EC_FUNC_HEADER;

   466

    datagram->type = EC_DATAGRAM_LRW;

   467

    EC_WRITE_U32(datagram->address, offset);

   468

    EC_FUNC_FOOTER;

   469

}

   470

   471

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

   472

   473

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

   474

 *

   475

 * Outputs an error message.

   476

 */

   477

void ec_datagram_print_wc_error(

   478

        const ec_datagram_t *datagram /**< EtherCAT datagram */

   479

        )

   480

{

   481

    if (datagram->working_counter == 0)

   482

        printk("No response.");

   483

    else if (datagram->working_counter > 1)

   484

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

   485

    else

   486

        printk("Success.");

   487

    printk("\n");

   488

}

   489

   490

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

   491