1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/**

    28

   \file

    29

   Methods of an EtherCAT datagram.

    30

*/

    31

    32

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

    33

    34

#include <linux/slab.h>

    35

    36

#include "datagram.h"

    37

#include "master.h"

    38

    39

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

    40

    41

/** \cond */

    42

    43

#define EC_FUNC_HEADER \

    44

    ret = ec_datagram_prealloc(datagram, data_size); \

    45

    if (unlikely(ret)) \

    46

        return ret; \

    47

    datagram->index = 0; \

    48

    datagram->working_counter = 0; \

    49

    datagram->state = EC_DATAGRAM_INIT;

    50

    51

#define EC_FUNC_FOOTER \

    52

    datagram->data_size = data_size; \

    53

    return 0;

    54

    55

/** \endcond */

    56

    57

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

    58

    59

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

    60

 *

    61

 * \attention This is indexed by ec_datagram_type_t.

    62

 */

    63

static const char *type_strings[] = {

    64

    "?",

    65

    "APRD",

    66

    "APWR",

    67

    "APRW",

    68

    "FPRD",

    69

    "FPWR",

    70

    "FPRW",

    71

    "BRD",

    72

    "BWR",

    73

    "BRW",

    74

    "LRD",

    75

    "LWR",

    76

    "LRW",

    77

    "ARMW",

    78

    "FRMW"

    79

};

    80

    81

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

    82

    83

/** Constructor.

    84

 */

    85

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

    86

{

    87

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

    88

    datagram->type = EC_DATAGRAM_NONE;

    89

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

    90

    datagram->data = NULL;

    91

    datagram->data_origin = EC_ORIG_INTERNAL;

    92

    datagram->mem_size = 0;

    93

    datagram->data_size = 0;

    94

    datagram->index = 0x00;

    95

    datagram->working_counter = 0x0000;

    96

    datagram->state = EC_DATAGRAM_INIT;

    97

#ifdef EC_HAVE_CYCLES

    98

    datagram->cycles_sent = 0;

    99

#endif

   100

    datagram->jiffies_sent = 0;

   101

#ifdef EC_HAVE_CYCLES

   102

    datagram->cycles_received = 0;

   103

#endif

   104

    datagram->jiffies_received = 0;

   105

    datagram->skip_count = 0;

   106

    datagram->stats_output_jiffies = 0;

   107

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

   108

}

   109

   110

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

   111

   112

/** Destructor.

   113

 */

   114

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

   115

{

   116

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

   117

        kfree(datagram->data);

   118

    	datagram->data = NULL;

   119

    }

   120

}

   121

   122

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

   123

   124

/** Allocates internal payload memory.

   125

 *

   126

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

   127

 *

   128

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

   129

 *            is done!

   130

 *

   131

 * \return 0 in case of success, otherwise \a -ENOMEM.

   132

 */

   133

int ec_datagram_prealloc(

   134

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

   135

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

   136

        )

   137

{

   138

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   139

            || size <= datagram->mem_size)

   140

        return 0;

   141

   142

    if (datagram->data) {

   143

        kfree(datagram->data);

   144

        datagram->data = NULL;

   145

        datagram->mem_size = 0;

   146

    }

   147

   148

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

   149

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

   150

        return -ENOMEM;

   151

    }

   152

   153

    datagram->mem_size = size;

   154

    return 0;

   155

}

   156

   157

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

   158

   159

/** Fills the datagram payload memory with zeros.

   160

 */

   161

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

   162

{

   163

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

   164

}

   165

   166

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

   167

   168

/** Initializes an EtherCAT APRD datagram.

   169

 *

   170

 * \return Return value of ec_datagram_prealloc().

   171

 */

   172

int ec_datagram_aprd(

   173

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

   174

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

   175

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

   176

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

   177

        )

   178

{

   179

    int ret;

   180

    EC_FUNC_HEADER;

   181

    datagram->type = EC_DATAGRAM_APRD;

   182

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

   183

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

   184

    EC_FUNC_FOOTER;

   185

}

   186

   187

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

   188

   189

/** Initializes an EtherCAT APWR datagram.

   190

 *

   191

 * \return Return value of ec_datagram_prealloc().

   192

 */

   193

int ec_datagram_apwr(

   194

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

   195

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

   196

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

   197

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

   198

        )

   199

{

   200

    int ret;

   201

    EC_FUNC_HEADER;

   202

    datagram->type = EC_DATAGRAM_APWR;

   203

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

   204

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

   205

    EC_FUNC_FOOTER;

   206

}

   207

   208

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

   209

   210

/** Initializes an EtherCAT APRW datagram.

   211

 *

   212

 * \return Return value of ec_datagram_prealloc().

   213

 */

   214

int ec_datagram_aprw(

   215

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

   216

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

   217

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

   218

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

   219

        )

   220

{

   221

    int ret;

   222

    EC_FUNC_HEADER;

   223

    datagram->type = EC_DATAGRAM_APRW;

   224

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

   225

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

   226

    EC_FUNC_FOOTER;

   227

}

   228

   229

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

   230

   231

/** Initializes an EtherCAT ARMW datagram.

   232

 *

   233

 * \return Return value of ec_datagram_prealloc().

   234

 */

   235

int ec_datagram_armw(

   236

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

   237

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

   238

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

   239

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

   240

        )

   241

{

   242

    int ret;

   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 Return value of ec_datagram_prealloc().

   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

    int ret;

   264

   265

    if (unlikely(configured_address == 0x0000))

   266

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

   267

   268

    EC_FUNC_HEADER;

   269

    datagram->type = EC_DATAGRAM_FPRD;

   270

    EC_WRITE_U16(datagram->address, configured_address);

   271

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

   272

    EC_FUNC_FOOTER;

   273

}

   274

   275

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

   276

   277

/** Initializes an EtherCAT FPWR datagram.

   278

 *

   279

 * \return Return value of ec_datagram_prealloc().

   280

 */

   281

int ec_datagram_fpwr(

   282

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

   283

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

   284

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

   285

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

   286

        )

   287

{

   288

    int ret;

   289

   290

    if (unlikely(configured_address == 0x0000))

   291

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

   292

   293

    EC_FUNC_HEADER;

   294

    datagram->type = EC_DATAGRAM_FPWR;

   295

    EC_WRITE_U16(datagram->address, configured_address);

   296

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

   297

    EC_FUNC_FOOTER;

   298

}

   299

   300

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

   301

   302

/** Initializes an EtherCAT FPRW datagram.

   303

 *

   304

 * \return Return value of ec_datagram_prealloc().

   305

 */

   306

int ec_datagram_fprw(

   307

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

   308

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

   309

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

   310

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

   311

        )

   312

{

   313

    int ret;

   314

   315

    if (unlikely(configured_address == 0x0000))

   316

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

   317

   318

    EC_FUNC_HEADER;

   319

    datagram->type = EC_DATAGRAM_FPRW;

   320

    EC_WRITE_U16(datagram->address, configured_address);

   321

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

   322

    EC_FUNC_FOOTER;

   323

}

   324

   325

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

   326

   327

/** Initializes an EtherCAT FRMW datagram.

   328

 *

   329

 * \return Return value of ec_datagram_prealloc().

   330

 */

   331

int ec_datagram_frmw(

   332

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

   333

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

   334

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

   335

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

   336

        )

   337

{

   338

    int ret;

   339

   340

    if (unlikely(configured_address == 0x0000))

   341

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

   342

   343

    EC_FUNC_HEADER;

   344

    datagram->type = EC_DATAGRAM_FRMW;

   345

    EC_WRITE_U16(datagram->address, configured_address);

   346

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

   347

    EC_FUNC_FOOTER;

   348

}

   349

   350

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

   351

   352

/** Initializes an EtherCAT BRD datagram.

   353

 *

   354

 * \return Return value of ec_datagram_prealloc().

   355

 */

   356

int ec_datagram_brd(

   357

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

   358

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

   359

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

   360

        )

   361

{

   362

    int ret;

   363

    EC_FUNC_HEADER;

   364

    datagram->type = EC_DATAGRAM_BRD;

   365

    EC_WRITE_U16(datagram->address, 0x0000);

   366

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

   367

    EC_FUNC_FOOTER;

   368

}

   369

   370

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

   371

   372

/** Initializes an EtherCAT BWR datagram.

   373

 *

   374

 * \return Return value of ec_datagram_prealloc().

   375

 */

   376

int ec_datagram_bwr(

   377

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

   378

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

   379

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

   380

        )

   381

{

   382

    int ret;

   383

    EC_FUNC_HEADER;

   384

    datagram->type = EC_DATAGRAM_BWR;

   385

    EC_WRITE_U16(datagram->address, 0x0000);

   386

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

   387

    EC_FUNC_FOOTER;

   388

}

   389

   390

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

   391

   392

/** Initializes an EtherCAT BRW datagram.

   393

 *

   394

 * \return Return value of ec_datagram_prealloc().

   395

 */

   396

int ec_datagram_brw(

   397

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

   398

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

   399

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

   400

        )

   401

{

   402

    int ret;

   403

    EC_FUNC_HEADER;

   404

    datagram->type = EC_DATAGRAM_BRW;

   405

    EC_WRITE_U16(datagram->address, 0x0000);

   406

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

   407

    EC_FUNC_FOOTER;

   408

}

   409

   410

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

   411

   412

/** Initializes an EtherCAT LRD datagram.

   413

 *

   414

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

   415

 *            bytes large.

   416

 *

   417

 * \return Return value of ec_datagram_prealloc().

   418

 */

   419

int ec_datagram_lrd(

   420

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

   421

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

   422

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

   423

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

   424

        )

   425

{

   426

    int ret;

   427

    datagram->data = external_memory;

   428

    datagram->data_origin = EC_ORIG_EXTERNAL;

   429

    EC_FUNC_HEADER;

   430

    datagram->type = EC_DATAGRAM_LRD;

   431

    EC_WRITE_U32(datagram->address, offset);

   432

    EC_FUNC_FOOTER;

   433

}

   434

   435

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

   436

   437

/** Initializes an EtherCAT LWR datagram.

   438

 *

   439

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

   440

 *            bytes large.

   441

 *

   442

 * \return Return value of ec_datagram_prealloc().

   443

 */

   444

int ec_datagram_lwr(

   445

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

   446

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

   447

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

   448

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

   449

        )

   450

{

   451

    int ret;

   452

    datagram->data = external_memory;

   453

    datagram->data_origin = EC_ORIG_EXTERNAL;

   454

    EC_FUNC_HEADER;

   455

    datagram->type = EC_DATAGRAM_LWR;

   456

    EC_WRITE_U32(datagram->address, offset);

   457

    EC_FUNC_FOOTER;

   458

}

   459

   460

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

   461

   462

/** Initializes an EtherCAT LRW datagram.

   463

 *

   464

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

   465

 *            bytes large.

   466

 *

   467

 * \return Return value of ec_datagram_prealloc().

   468

 */

   469

int ec_datagram_lrw(

   470

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

   471

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

   472

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

   473

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

   474

        )

   475

{

   476

    int ret;

   477

    datagram->data = external_memory;

   478

    datagram->data_origin = EC_ORIG_EXTERNAL;

   479

    EC_FUNC_HEADER;

   480

    datagram->type = EC_DATAGRAM_LRW;

   481

    EC_WRITE_U32(datagram->address, offset);

   482

    EC_FUNC_FOOTER;

   483

}

   484

   485

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

   486

   487

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

   488

 *