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; version 2 of the License.

    12

 *

    13

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

    14

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

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    16

 *  GNU General Public License for more details.

    17

 *

    18

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

    19

 *  along 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

/**

    25

   \file

    26

   Methods of an EtherCAT command.

    27

*/

    28

    29

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

    30

    31

#include <linux/slab.h>

    32

#include <linux/delay.h>

    33

    34

#include "command.h"

    35

#include "master.h"

    36

    37

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

    38

    39

/** \cond */

    40

    41

#define EC_FUNC_HEADER \

    42

    if (unlikely(ec_command_prealloc(command, data_size))) \

    43

        return -1; \

    44

    command->index = 0; \

    45

    command->working_counter = 0; \

    46

    command->state = EC_CMD_INIT;

    47

    48

#define EC_FUNC_FOOTER \

    49

    command->data_size = data_size; \

    50

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

    51

    return 0;

    52

    53

/** \endcond */

    54

    55

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

    56

    57

/**

    58

   Command constructor.

    59

*/

    60

    61

void ec_command_init(ec_command_t *command /**< EtherCAT command */)

    62

{

    63

    command->type = EC_CMD_NONE;

    64

    command->address.logical = 0x00000000;

    65

    command->data = NULL;

    66

    command->mem_size = 0;

    67

    command->data_size = 0;

    68

    command->index = 0x00;

    69

    command->working_counter = 0x00;

    70

    command->state = EC_CMD_INIT;

    71

    command->t_sent = 0;

    72

}

    73

    74

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

    75

    76

/**

    77

   Command destructor.

    78

*/

    79

    80

void ec_command_clear(ec_command_t *command /**< EtherCAT command */)

    81

{

    82

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

    83

}

    84

    85

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

    86

    87

/**

    88

   Allocates command data memory.

    89

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

    90

   \return 0 in case of success, else < 0

    91

*/

    92

    93

int ec_command_prealloc(ec_command_t *command, /**< EtherCAT command */

    94

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

    95

                        )

    96

{

    97

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

    98

    99

    if (command->data) {

   100

        kfree(command->data);

   101

        command->data = NULL;

   102

        command->mem_size = 0;

   103

    }

   104

   105

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

   106

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

   107

        return -1;

   108

    }

   109

   110

    command->mem_size = size;

   111

    return 0;

   112

}

   113

   114

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

   115

   116

/**

   117

   Initializes an EtherCAT NPRD command.

   118

   Node-adressed physical read.

   119

   \return 0 in case of success, else < 0

   120

*/

   121

   122

int ec_command_nprd(ec_command_t *command,

   123

                    /**< EtherCAT command */

   124

                    uint16_t node_address,

   125

                    /**< configured station address */

   126

                    uint16_t offset,

   127

                    /**< physical memory address */

   128

                    size_t data_size

   129

                    /**< number of bytes to read */

   130

                    )

   131

{

   132

    if (unlikely(node_address == 0x0000))

   133

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

   134

   135

    EC_FUNC_HEADER;

   136

    command->type = EC_CMD_NPRD;

   137

    command->address.physical.slave = node_address;

   138

    command->address.physical.mem = offset;

   139

    EC_FUNC_FOOTER;

   140

}

   141

   142

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

   143

   144

/**

   145

   Initializes an EtherCAT NPWR command.

   146

   Node-adressed physical write.

   147

   \return 0 in case of success, else < 0

   148

*/

   149

   150

int ec_command_npwr(ec_command_t *command,

   151

                    /**< EtherCAT command */

   152

                    uint16_t node_address,

   153

                    /**< configured station address */

   154

                    uint16_t offset,

   155

                    /**< physical memory address */

   156

                    size_t data_size

   157

                    /**< number of bytes to write */

   158

                    )

   159

{

   160

    if (unlikely(node_address == 0x0000))

   161

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

   162

   163

    EC_FUNC_HEADER;

   164

    command->type = EC_CMD_NPWR;

   165

    command->address.physical.slave = node_address;

   166

    command->address.physical.mem = offset;

   167

    EC_FUNC_FOOTER;

   168

}

   169

   170

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

   171

   172

/**

   173

   Initializes an EtherCAT APRD command.

   174

   Autoincrement physical read.

   175

   \return 0 in case of success, else < 0

   176

*/

   177

   178

int ec_command_aprd(ec_command_t *command,

   179

                    /**< EtherCAT command */

   180

                    uint16_t ring_position,

   181

                    /**< auto-increment position */

   182

                    uint16_t offset,

   183

                    /**< physical memory address */

   184

                    size_t data_size

   185

                    /**< number of bytes to read */

   186

                    )

   187

{

   188

    EC_FUNC_HEADER;

   189

    command->type = EC_CMD_APRD;

   190

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

   191

    command->address.physical.mem = offset;

   192

    EC_FUNC_FOOTER;

   193

}

   194

   195

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

   196

   197

/**

   198

   Initializes an EtherCAT APWR command.

   199

   Autoincrement physical write.

   200

   \return 0 in case of success, else < 0

   201

*/

   202

   203

int ec_command_apwr(ec_command_t *command,

   204

                    /**< EtherCAT command */

   205

                    uint16_t ring_position,

   206

                    /**< auto-increment position */

   207

                    uint16_t offset,

   208

                    /**< physical memory address */

   209

                    size_t data_size

   210

                    /**< number of bytes to write */

   211

                    )

   212

{

   213

    EC_FUNC_HEADER;

   214

    command->type = EC_CMD_APWR;

   215

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

   216

    command->address.physical.mem = offset;

   217

    EC_FUNC_FOOTER;

   218

}

   219

   220

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

   221

   222

/**

   223

   Initializes an EtherCAT BRD command.

   224

   Broadcast read.

   225

   \return 0 in case of success, else < 0

   226

*/

   227

   228

int ec_command_brd(ec_command_t *command,

   229

                   /**< EtherCAT command */

   230

                   uint16_t offset,

   231

                   /**< physical memory address */

   232

                   size_t data_size

   233

                   /**< number of bytes to read */

   234

                   )

   235

{

   236

    EC_FUNC_HEADER;

   237

    command->type = EC_CMD_BRD;

   238

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

   239

    command->address.physical.mem = offset;

   240

    EC_FUNC_FOOTER;

   241

}

   242

   243

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

   244

   245

/**

   246

   Initializes an EtherCAT BWR command.

   247

   Broadcast write.

   248

   \return 0 in case of success, else < 0

   249

*/

   250

   251

int ec_command_bwr(ec_command_t *command,

   252

                   /**< EtherCAT command */

   253

                   uint16_t offset,

   254

                   /**< physical memory address */

   255

                   size_t data_size

   256

                   /**< number of bytes to write */

   257

                   )

   258

{

   259

    EC_FUNC_HEADER;

   260

    command->type = EC_CMD_BWR;

   261

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

   262

    command->address.physical.mem = offset;

   263

    EC_FUNC_FOOTER;

   264

}

   265

   266

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

   267

   268

/**

   269

   Initializes an EtherCAT LRW command.

   270

   Logical read write.

   271

   \return 0 in case of success, else < 0

   272

*/

   273

   274

int ec_command_lrw(ec_command_t *command,

   275

                   /**< EtherCAT command */

   276

                   uint32_t offset,

   277

                   /**< logical address */

   278

                   size_t data_size

   279

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

   280

                   )

   281

{

   282

    EC_FUNC_HEADER;

   283

    command->type = EC_CMD_LRW;

   284

    command->address.logical = offset;

   285

    EC_FUNC_FOOTER;

   286

}

   287

   288

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