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

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

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

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/**

    31

   \file

    32

   Methods of an EtherCAT datagram.

    33

*/

    34

    35

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

    36

    37

#include <linux/slab.h>

    38

    39

#include "datagram.h"

    40

#include "master.h"

    41

    42

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

    43

    44

/** \cond */

    45

    46

#define EC_FUNC_HEADER \

    47

    ret = ec_datagram_prealloc(datagram, data_size); \

    48

    if (unlikely(ret)) \

    49

        return ret; \

    50

    datagram->index = 0; \

    51

    datagram->working_counter = 0; \

    52

    datagram->state = EC_DATAGRAM_INIT;

    53

    54

#define EC_FUNC_FOOTER \

    55

    datagram->data_size = data_size; \

    56

    return 0;

    57

    58

/** \endcond */

    59

    60

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

    61

    62

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

    63

 *

    64

 * \attention This is indexed by ec_datagram_type_t.

    65

 */

    66

static const char *type_strings[] = {

    67

    "?",

    68

    "APRD",

    69

    "APWR",

    70

    "APRW",

    71

    "FPRD",

    72

    "FPWR",

    73

    "FPRW",

    74

    "BRD",

    75

    "BWR",

    76

    "BRW",

    77

    "LRD",

    78

    "LWR",

    79

    "LRW",

    80

    "ARMW",

    81

    "FRMW"

    82

};

    83

    84

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

    85

    86

/** Constructor.

    87

 */

    88

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

    89

{

    90

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

    91

    datagram->type = EC_DATAGRAM_NONE;

    92

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

    93

    datagram->data = NULL;

    94

    datagram->data_origin = EC_ORIG_INTERNAL;

    95

    datagram->mem_size = 0;

    96

    datagram->data_size = 0;

    97

    datagram->index = 0x00;

    98

    datagram->working_counter = 0x0000;

    99

    datagram->state = EC_DATAGRAM_INIT;

   100

#ifdef EC_HAVE_CYCLES

   101

    datagram->cycles_sent = 0;

   102

#endif

   103

    datagram->jiffies_sent = 0;

   104

#ifdef EC_HAVE_CYCLES

   105

    datagram->cycles_received = 0;

   106

#endif

   107

    datagram->jiffies_received = 0;

   108

    datagram->skip_count = 0;

   109

    datagram->stats_output_jiffies = 0;

   110

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

   111

}

   112

   113

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

   114

   115

/** Destructor.

   116

 */

   117

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

   118

{

   119

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

   120

        kfree(datagram->data);

   121

    	datagram->data = NULL;

   122

    }

   123

}

   124

   125

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

   126

   127

/** Allocates internal payload memory.

   128

 *

   129

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

   130

 *

   131

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

   132

 *            is done!

   133

 *

   134

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

   135

 */

   136

int ec_datagram_prealloc(

   137

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

   138

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

   139

        )

   140

{

   141

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   142

            || size <= datagram->mem_size)

   143

        return 0;

   144

   145

    if (datagram->data) {

   146

        kfree(datagram->data);

   147

        datagram->data = NULL;

   148

        datagram->mem_size = 0;

   149

    }

   150

   151

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

   152

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

   153

        return -ENOMEM;

   154

    }

   155

   156

    datagram->mem_size = size;

   157

    return 0;

   158

}

   159

   160

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

   161

   162

/** Fills the datagram payload memory with zeros.

   163

 */

   164

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

   165

{

   166

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

   167

}

   168

   169

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

   170

   171

/** Initializes an EtherCAT APRD datagram.

   172

 *

   173

 * \return Return value of ec_datagram_prealloc().

   174

 */

   175

int ec_datagram_aprd(

   176

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

   177

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

   178

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

   179

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

   180

        )

   181

{

   182

    int ret;

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

   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

    int ret;

   204

    EC_FUNC_HEADER;

   205

    datagram->type = EC_DATAGRAM_APWR;

   206

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

   207

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

   208

    EC_FUNC_FOOTER;

   209

}

   210

   211

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

   212

   213

/** Initializes an EtherCAT APRW datagram.

   214

 *

   215

 * \return Return value of ec_datagram_prealloc().

   216

 */

   217

int ec_datagram_aprw(

   218

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

   219

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

   220

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

   221

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

   222

        )

   223

{

   224

    int ret;

   225

    EC_FUNC_HEADER;

   226

    datagram->type = EC_DATAGRAM_APRW;

   227

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

   228

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

   229

    EC_FUNC_FOOTER;

   230

}

   231

   232

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

   233

   234

/** Initializes an EtherCAT ARMW datagram.

   235

 *

   236

 * \return Return value of ec_datagram_prealloc().

   237

 */

   238

int ec_datagram_armw(

   239

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

   240

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

   241

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

   242

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

   243

        )

   244

{

   245

    int ret;

   246

    EC_FUNC_HEADER;

   247

    datagram->type = EC_DATAGRAM_ARMW;

   248

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

   249

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

   250

    EC_FUNC_FOOTER;

   251

}

   252

   253

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

   254

   255

/** Initializes an EtherCAT FPRD datagram.

   256

 *

   257

 * \return Return value of ec_datagram_prealloc().

   258

 */

   259

int ec_datagram_fprd(

   260

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

   261

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

   262

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

   263

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

   264

        )

   265

{

   266

    int ret;

   267

   268

    if (unlikely(configured_address == 0x0000))

   269

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

   270

   271

    EC_FUNC_HEADER;

   272

    datagram->type = EC_DATAGRAM_FPRD;

   273

    EC_WRITE_U16(datagram->address, configured_address);

   274

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

   275

    EC_FUNC_FOOTER;

   276

}

   277

   278

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

   279

   280

/** Initializes an EtherCAT FPWR datagram.

   281

 *

   282

 * \return Return value of ec_datagram_prealloc().

   283

 */

   284

int ec_datagram_fpwr(

   285

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

   286

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

   287

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

   288

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

   289

        )

   290

{

   291

    int ret;

   292

   293

    if (unlikely(configured_address == 0x0000))

   294

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

   295

   296

    EC_FUNC_HEADER;

   297

    datagram->type = EC_DATAGRAM_FPWR;

   298

    EC_WRITE_U16(datagram->address, configured_address);

   299

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

   300

    EC_FUNC_FOOTER;

   301

}

   302

   303

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

   304

   305

/** Initializes an EtherCAT FPRW datagram.

   306

 *

   307

 * \return Return value of ec_datagram_prealloc().

   308

 */

   309

int ec_datagram_fprw(

   310

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

   311

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

   312

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

   313

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

   314

        )

   315

{

   316

    int ret;

   317

   318

    if (unlikely(configured_address == 0x0000))

   319

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

   320

   321

    EC_FUNC_HEADER;

   322

    datagram->type = EC_DATAGRAM_FPRW;

   323

    EC_WRITE_U16(datagram->address, configured_address);

   324

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

   325

    EC_FUNC_FOOTER;

   326

}

   327

   328

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

   329

   330

/** Initializes an EtherCAT FRMW datagram.

   331

 *

   332

 * \return Return value of ec_datagram_prealloc().

   333

 */

   334

int ec_datagram_frmw(

   335

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

   336

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

   337

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

   338

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

   339

        )

   340

{

   341

    int ret;

   342

   343

    if (unlikely(configured_address == 0x0000))

   344

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

   345

   346

    EC_FUNC_HEADER;

   347

    datagram->type = EC_DATAGRAM_FRMW;

   348

    EC_WRITE_U16(datagram->address, configured_address);

   349

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

   350

    EC_FUNC_FOOTER;

   351

}

   352

   353

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

   354

   355

/** Initializes an EtherCAT BRD datagram.

   356

 *

   357

 * \return Return value of ec_datagram_prealloc().

   358

 */

   359

int ec_datagram_brd(

   360

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

   361

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

   362

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

   363

        )

   364

{

   365

    int ret;

   366

    EC_FUNC_HEADER;

   367

    datagram->type = EC_DATAGRAM_BRD;

   368

    EC_WRITE_U16(datagram->address, 0x0000);

   369

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

   370

    EC_FUNC_FOOTER;

   371

}

   372

   373

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

   374

   375

/** Initializes an EtherCAT BWR datagram.

   376

 *

   377

 * \return Return value of ec_datagram_prealloc().

   378

 */

   379

int ec_datagram_bwr(

   380

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

   381

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

   382

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

   383

        )

   384

{

   385

    int ret;

   386

    EC_FUNC_HEADER;

   387

    datagram->type = EC_DATAGRAM_BWR;

   388

    EC_WRITE_U16(datagram->address, 0x0000);

   389

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

   390

    EC_FUNC_FOOTER;

   391

}

   392

   393

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

   394

   395

/** Initializes an EtherCAT BRW datagram.

   396

 *

   397

 * \return Return value of ec_datagram_prealloc().

   398

 */

   399

int ec_datagram_brw(

   400

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

   401

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

   402

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

   403

        )

   404

{

   405

    int ret;

   406

    EC_FUNC_HEADER;

   407

    datagram->type = EC_DATAGRAM_BRW;

   408

    EC_WRITE_U16(datagram->address, 0x0000);

   409

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

   410

    EC_FUNC_FOOTER;

   411

}

   412

   413

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

   414

   415

/** Initializes an EtherCAT LRD datagram.

   416

 *

   417

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

   418

 *            bytes large.

   419

 *

   420

 * \return Return value of ec_datagram_prealloc().

   421

 */

   422

int ec_datagram_lrd(

   423

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

   424

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

   425

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

   426

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

   427

        )

   428

{

   429

    int ret;

   430

    datagram->data = external_memory;

   431

    datagram->data_origin = EC_ORIG_EXTERNAL;

   432

    EC_FUNC_HEADER;

   433

    datagram->type = EC_DATAGRAM_LRD;

   434

    EC_WRITE_U32(datagram->address, offset);

   435

    EC_FUNC_FOOTER;

   436

}

   437

   438

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

   439

   440

/** Initializes an EtherCAT LWR datagram.

   441

 *

   442

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

   443

 *            bytes large.

   444

 *

   445

 * \return Return value of ec_datagram_prealloc().

   446

 */

   447

int ec_datagram_lwr(

   448

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

   449

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

   450

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

   451

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

   452

        )

   453

{

   454

    int ret;

   455

    datagram->data = external_memory;

   456

    datagram->data_origin = EC_ORIG_EXTERNAL;

   457

    EC_FUNC_HEADER;

   458

    datagram->type = EC_DATAGRAM_LWR;

   459

    EC_WRITE_U32(datagram->address, offset);

   460

    EC_FUNC_FOOTER;

   461

}

   462

   463

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

   464

   465

/** Initializes an EtherCAT LRW datagram.

   466

 *

   467

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

   468

 *            bytes large.

   469

 *

   470

 * \return Return value of ec_datagram_prealloc().

   471

 */

   472

int ec_datagram_lrw(

   473

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

   474

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

   475

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

   476

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

   477

        )

   478

{

   479

    int ret;

   480

    datagram->data = external_memory;

   481

    datagram->data_origin = EC_ORIG_EXTERNAL;

   482

    EC_FUNC_HEADER;

   483

    datagram->type = EC_DATAGRAM_LRW;

   484

    EC_WRITE_U32(datagram->address, offset);

   485

    EC_FUNC_FOOTER;

   486

}

   487

   488

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