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_CMD_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

    datagram->type = EC_CMD_NONE;

    73

    datagram->address.logical = 0x00000000;

    74

    datagram->data = NULL;

    75

    datagram->mem_size = 0;

    76

    datagram->data_size = 0;

    77

    datagram->index = 0x00;

    78

    datagram->working_counter = 0x00;

    79

    datagram->state = EC_CMD_INIT;

    80

    datagram->t_sent = 0;

    81

}

    82

    83

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

    84

    85

/**

    86

   Datagram destructor.

    87

*/

    88

    89

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

    90

{

    91

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

    92

}

    93

    94

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

    95

    96

/**

    97

   Allocates datagram data memory.

    98

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

    99

   \return 0 in case of success, else < 0

   100

*/

   101

   102

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

   103

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

   104

                         )

   105

{

   106

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

   107

   108

    if (datagram->data) {

   109

        kfree(datagram->data);

   110

        datagram->data = NULL;

   111

        datagram->mem_size = 0;

   112

    }

   113

   114

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

   115

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

   116

        return -1;

   117

    }

   118

   119

    datagram->mem_size = size;

   120

    return 0;

   121

}

   122

   123

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

   124

   125

/**

   126

   Initializes an EtherCAT NPRD datagram.

   127

   Node-adressed physical read.

   128

   \return 0 in case of success, else < 0

   129

*/

   130

   131

int ec_datagram_nprd(ec_datagram_t *datagram,

   132

                     /**< EtherCAT datagram */

   133

                     uint16_t node_address,

   134

                     /**< configured station address */

   135

                     uint16_t offset,

   136

                     /**< physical memory address */

   137

                     size_t data_size

   138

                     /**< number of bytes to read */

   139

                     )

   140

{

   141

    if (unlikely(node_address == 0x0000))

   142

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

   143

   144

    EC_FUNC_HEADER;

   145

    datagram->type = EC_CMD_NPRD;

   146

    datagram->address.physical.slave = node_address;

   147

    datagram->address.physical.mem = offset;

   148

    EC_FUNC_FOOTER;

   149

}

   150

   151

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

   152

   153

/**

   154

   Initializes an EtherCAT NPWR datagram.

   155

   Node-adressed physical write.

   156

   \return 0 in case of success, else < 0

   157

*/

   158

   159

int ec_datagram_npwr(ec_datagram_t *datagram,

   160

                     /**< EtherCAT datagram */

   161

                     uint16_t node_address,

   162

                     /**< configured station address */

   163

                     uint16_t offset,

   164

                     /**< physical memory address */

   165

                     size_t data_size

   166

                     /**< number of bytes to write */

   167

                     )

   168

{

   169

    if (unlikely(node_address == 0x0000))

   170

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

   171

   172

    EC_FUNC_HEADER;

   173

    datagram->type = EC_CMD_NPWR;

   174

    datagram->address.physical.slave = node_address;

   175

    datagram->address.physical.mem = offset;

   176

    EC_FUNC_FOOTER;

   177

}

   178

   179

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

   180

   181

/**

   182

   Initializes an EtherCAT APRD datagram.

   183

   Autoincrement physical read.

   184

   \return 0 in case of success, else < 0

   185

*/

   186

   187

int ec_datagram_aprd(ec_datagram_t *datagram,

   188

                     /**< EtherCAT datagram */

   189

                     uint16_t ring_position,

   190

                     /**< auto-increment position */

   191

                     uint16_t offset,

   192

                     /**< physical memory address */

   193

                     size_t data_size

   194

                     /**< number of bytes to read */

   195

                     )

   196

{

   197

    EC_FUNC_HEADER;

   198

    datagram->type = EC_CMD_APRD;

   199

    datagram->address.physical.slave = (int16_t) ring_position * (-1);

   200

    datagram->address.physical.mem = offset;

   201

    EC_FUNC_FOOTER;

   202

}

   203

   204

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

   205

   206

/**

   207

   Initializes an EtherCAT APWR datagram.

   208

   Autoincrement physical write.

   209

   \return 0 in case of success, else < 0

   210

*/

   211

   212

int ec_datagram_apwr(ec_datagram_t *datagram,

   213

                     /**< EtherCAT datagram */

   214

                     uint16_t ring_position,

   215

                     /**< auto-increment position */

   216

                     uint16_t offset,

   217

                     /**< physical memory address */

   218

                     size_t data_size

   219

                     /**< number of bytes to write */

   220

                     )

   221

{

   222

    EC_FUNC_HEADER;

   223

    datagram->type = EC_CMD_APWR;

   224

    datagram->address.physical.slave = (int16_t) ring_position * (-1);

   225

    datagram->address.physical.mem = offset;

   226

    EC_FUNC_FOOTER;

   227

}

   228

   229

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

   230

   231

/**

   232

   Initializes an EtherCAT BRD datagram.

   233

   Broadcast read.

   234

   \return 0 in case of success, else < 0

   235

*/

   236

   237

int ec_datagram_brd(ec_datagram_t *datagram,

   238

                    /**< EtherCAT datagram */

   239

                    uint16_t offset,

   240

                    /**< physical memory address */

   241

                    size_t data_size

   242

                    /**< number of bytes to read */

   243

                    )

   244

{

   245

    EC_FUNC_HEADER;

   246

    datagram->type = EC_CMD_BRD;

   247

    datagram->address.physical.slave = 0x0000;

   248

    datagram->address.physical.mem = offset;

   249

    EC_FUNC_FOOTER;

   250

}

   251

   252

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

   253

   254

/**

   255

   Initializes an EtherCAT BWR datagram.

   256

   Broadcast write.

   257

   \return 0 in case of success, else < 0

   258

*/

   259

   260

int ec_datagram_bwr(ec_datagram_t *datagram,

   261

                    /**< EtherCAT datagram */

   262

                    uint16_t offset,

   263

                    /**< physical memory address */

   264

                    size_t data_size

   265

                    /**< number of bytes to write */

   266

                    )

   267

{

   268

    EC_FUNC_HEADER;

   269

    datagram->type = EC_CMD_BWR;

   270

    datagram->address.physical.slave = 0x0000;

   271

    datagram->address.physical.mem = offset;

   272

    EC_FUNC_FOOTER;

   273

}

   274

   275

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

   276

   277

/**

   278

   Initializes an EtherCAT LRW datagram.

   279

   Logical read write.

   280

   \return 0 in case of success, else < 0

   281

*/

   282

   283

int ec_datagram_lrw(ec_datagram_t *datagram,

   284

                    /**< EtherCAT datagram */

   285

                    uint32_t offset,

   286

                    /**< logical address */

   287

                    size_t data_size

   288

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

   289

                    )

   290

{

   291

    EC_FUNC_HEADER;

   292

    datagram->type = EC_CMD_LRW;

   293

    datagram->address.logical = offset;

   294

    EC_FUNC_FOOTER;

   295

}

   296

   297

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