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

    33

#include "command.h"

    34

#include "master.h"

    35

    36

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

    37

    38

/** \cond */

    39

    40

#define EC_FUNC_HEADER \

    41

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

    42

        return -1; \

    43

    command->index = 0; \

    44

    command->working_counter = 0; \

    45

    command->state = EC_CMD_INIT;

    46

    47

#define EC_FUNC_FOOTER \

    48

    command->data_size = data_size; \

    49

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

    50

    return 0;

    51

    52

/** \endcond */

    53

    54

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

    55

    56

/**

    57

   Command constructor.

    58

*/

    59

    60

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

    61

{

    62

    command->type = EC_CMD_NONE;

    63

    command->address.logical = 0x00000000;

    64

    command->data = NULL;

    65

    command->mem_size = 0;

    66

    command->data_size = 0;

    67

    command->index = 0x00;

    68

    command->working_counter = 0x00;

    69

    command->state = EC_CMD_INIT;

    70

    command->t_sent = 0;

    71

}

    72

    73

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

    74

    75

/**

    76

   Command destructor.

    77

*/

    78

    79

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

    80

{

    81

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

    82

}

    83

    84

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

    85

    86

/**

    87

   Allocates command data memory.

    88

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

    89

   \return 0 in case of success, else < 0

    90

*/

    91

    92

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

    93

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

    94

                        )

    95

{

    96

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

    97

    98

    if (command->data) {

    99

        kfree(command->data);

   100

        command->data = NULL;

   101

        command->mem_size = 0;

   102

    }

   103

   104

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

   105

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

   106

        return -1;

   107

    }

   108

   109

    command->mem_size = size;

   110

    return 0;

   111

}

   112

   113

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

   114

   115

/**

   116

   Initializes an EtherCAT NPRD command.

   117

   Node-adressed physical read.

   118

   \return 0 in case of success, else < 0

   119

*/

   120

   121

int ec_command_nprd(ec_command_t *command,

   122

                    /**< EtherCAT command */

   123

                    uint16_t node_address,

   124

                    /**< configured station address */

   125

                    uint16_t offset,

   126

                    /**< physical memory address */

   127

                    size_t data_size

   128

                    /**< number of bytes to read */

   129

                    )

   130

{

   131

    if (unlikely(node_address == 0x0000))

   132

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

   133

   134

    EC_FUNC_HEADER;

   135

    command->type = EC_CMD_NPRD;

   136

    command->address.physical.slave = node_address;

   137

    command->address.physical.mem = offset;

   138

    EC_FUNC_FOOTER;

   139

}

   140

   141

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

   142

   143

/**

   144

   Initializes an EtherCAT NPWR command.

   145

   Node-adressed physical write.

   146

   \return 0 in case of success, else < 0

   147

*/

   148

   149

int ec_command_npwr(ec_command_t *command,

   150

                    /**< EtherCAT command */

   151

                    uint16_t node_address,

   152

                    /**< configured station address */

   153

                    uint16_t offset,

   154

                    /**< physical memory address */

   155

                    size_t data_size

   156

                    /**< number of bytes to write */

   157

                    )

   158

{

   159

    if (unlikely(node_address == 0x0000))

   160

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

   161

   162

    EC_FUNC_HEADER;

   163

    command->type = EC_CMD_NPWR;

   164

    command->address.physical.slave = node_address;

   165

    command->address.physical.mem = offset;

   166

    EC_FUNC_FOOTER;

   167

}

   168

   169

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

   170

   171

/**

   172

   Initializes an EtherCAT APRD command.

   173

   Autoincrement physical read.

   174

   \return 0 in case of success, else < 0

   175

*/

   176

   177

int ec_command_aprd(ec_command_t *command,

   178

                    /**< EtherCAT command */

   179

                    uint16_t ring_position,

   180

                    /**< auto-increment position */

   181

                    uint16_t offset,

   182

                    /**< physical memory address */

   183

                    size_t data_size

   184

                    /**< number of bytes to read */

   185

                    )

   186

{

   187

    EC_FUNC_HEADER;

   188

    command->type = EC_CMD_APRD;

   189

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

   190

    command->address.physical.mem = offset;

   191

    EC_FUNC_FOOTER;

   192

}

   193

   194

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

   195

   196

/**

   197

   Initializes an EtherCAT APWR command.

   198

   Autoincrement physical write.

   199

   \return 0 in case of success, else < 0

   200

*/

   201

   202

int ec_command_apwr(ec_command_t *command,

   203

                    /**< EtherCAT command */

   204

                    uint16_t ring_position,

   205

                    /**< auto-increment position */

   206

                    uint16_t offset,

   207

                    /**< physical memory address */

   208

                    size_t data_size

   209

                    /**< number of bytes to write */

   210

                    )

   211

{

   212

    EC_FUNC_HEADER;

   213

    command->type = EC_CMD_APWR;

   214

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

   215

    command->address.physical.mem = offset;

   216

    EC_FUNC_FOOTER;

   217

}

   218

   219

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

   220

   221

/**

   222

   Initializes an EtherCAT BRD command.

   223

   Broadcast read.

   224

   \return 0 in case of success, else < 0

   225

*/

   226

   227

int ec_command_brd(ec_command_t *command,

   228

                   /**< EtherCAT command */

   229

                   uint16_t offset,

   230

                   /**< physical memory address */

   231

                   size_t data_size

   232

                   /**< number of bytes to read */

   233

                   )

   234

{

   235

    EC_FUNC_HEADER;

   236

    command->type = EC_CMD_BRD;

   237

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

   238

    command->address.physical.mem = offset;

   239

    EC_FUNC_FOOTER;

   240

}

   241

   242

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

   243

   244

/**

   245

   Initializes an EtherCAT BWR command.

   246

   Broadcast write.

   247

   \return 0 in case of success, else < 0

   248

*/

   249

   250

int ec_command_bwr(ec_command_t *command,

   251

                   /**< EtherCAT command */

   252

                   uint16_t offset,

   253

                   /**< physical memory address */

   254

                   size_t data_size

   255

                   /**< number of bytes to write */

   256

                   )

   257

{

   258

    EC_FUNC_HEADER;

   259

    command->type = EC_CMD_BWR;

   260

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

   261

    command->address.physical.mem = offset;

   262

    EC_FUNC_FOOTER;

   263

}

   264

   265

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

   266

   267

/**

   268

   Initializes an EtherCAT LRW command.

   269

   Logical read write.

   270

   \return 0 in case of success, else < 0

   271

*/

   272

   273

int ec_command_lrw(ec_command_t *command,

   274

                   /**< EtherCAT command */

   275

                   uint32_t offset,

   276

                   /**< logical address */

   277

                   size_t data_size

   278

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

   279

                   )

   280

{

   281

    EC_FUNC_HEADER;

   282

    command->type = EC_CMD_LRW;

   283

    command->address.logical = offset;

   284

    EC_FUNC_FOOTER;

   285

}

   286

   287

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