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

    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

 *

    32

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

    33

    34

/**

    35

   \file

    36

   Methods of an EtherCAT command.

    37

*/

    38

    39

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

    40

    41

#include <linux/slab.h>

    42

    43

#include "command.h"

    44

#include "master.h"

    45

    46

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

    47

    48

/** \cond */

    49

    50

#define EC_FUNC_HEADER \

    51

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

    52

        return -1; \

    53

    command->index = 0; \

    54

    command->working_counter = 0; \

    55

    command->state = EC_CMD_INIT;

    56

    57

#define EC_FUNC_FOOTER \

    58

    command->data_size = data_size; \

    59

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

    60

    return 0;

    61

    62

/** \endcond */

    63

    64

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

    65

    66

/**

    67

   Command constructor.

    68

*/

    69

    70

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

    71

{

    72

    command->type = EC_CMD_NONE;

    73

    command->address.logical = 0x00000000;

    74

    command->data = NULL;

    75

    command->mem_size = 0;

    76

    command->data_size = 0;

    77

    command->index = 0x00;

    78

    command->working_counter = 0x00;

    79

    command->state = EC_CMD_INIT;

    81

}

    82

    83

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

    84

    85

/**

    86

   Command destructor.

    87

*/

    88

    89

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

    90

{

    91

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

    92

}

    93

    94

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

    95

    96

/**

    97

   Allocates command 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_command_prealloc(ec_command_t *command, /**< EtherCAT command */

   103

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

   104

                        )

   105

{

   106

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

   107

   108

    if (command->data) {

   109

        kfree(command->data);

   110

        command->data = NULL;

   111

        command->mem_size = 0;

   112

    }

   113

   114

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

   115

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

   116

        return -1;

   117

    }

   118

   119

    command->mem_size = size;

   120

    return 0;

   121

}

   122

   123

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

   124

   125

/**

   126

   Initializes an EtherCAT NPRD command.

   127

   Node-adressed physical read.

   128

   \return 0 in case of success, else < 0

   129

*/

   130

   131

int ec_command_nprd(ec_command_t *command,

   132

                    /**< EtherCAT command */

   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

    command->type = EC_CMD_NPRD;

   146

    command->address.physical.slave = node_address;

   147

    command->address.physical.mem = offset;

   148

    EC_FUNC_FOOTER;

   149

}

   150

   151

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

   152

   153

/**

   154

   Initializes an EtherCAT NPWR command.

   155

   Node-adressed physical write.

   156

   \return 0 in case of success, else < 0

   157

*/

   158

   159

int ec_command_npwr(ec_command_t *command,

   160

                    /**< EtherCAT command */

   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

    command->type = EC_CMD_NPWR;

   174

    command->address.physical.slave = node_address;

   175

    command->address.physical.mem = offset;

   176

    EC_FUNC_FOOTER;

   177

}

   178

   179

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

   180

   181

/**

   182

   Initializes an EtherCAT APRD command.

   183

   Autoincrement physical read.

   184

   \return 0 in case of success, else < 0

   185

*/

   186

   187

int ec_command_aprd(ec_command_t *command,

   188

                    /**< EtherCAT command */

   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

    command->type = EC_CMD_APRD;

   199

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

   200

    command->address.physical.mem = offset;

   201

    EC_FUNC_FOOTER;

   202

}

   203

   204

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

   205

   206

/**

   207

   Initializes an EtherCAT APWR command.

   208

   Autoincrement physical write.

   209

   \return 0 in case of success, else < 0

   210

*/

   211

   212

int ec_command_apwr(ec_command_t *command,

   213

                    /**< EtherCAT command */

   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

    command->type = EC_CMD_APWR;

   224

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

   225

    command->address.physical.mem = offset;

   226

    EC_FUNC_FOOTER;

   227

}

   228

   229

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

   230

   231

/**

   232

   Initializes an EtherCAT BRD command.

   233

   Broadcast read.

   234

   \return 0 in case of success, else < 0

   235

*/

   236

   237

int ec_command_brd(ec_command_t *command,

   238

                   /**< EtherCAT command */

   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

    command->type = EC_CMD_BRD;

   247

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

   248

    command->address.physical.mem = offset;

   249

    EC_FUNC_FOOTER;

   250

}

   251

   252

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

   253

   254

/**

   255

   Initializes an EtherCAT BWR command.

   256

   Broadcast write.

   257

   \return 0 in case of success, else < 0

   258

*/

   259

   260

int ec_command_bwr(ec_command_t *command,

   261

                   /**< EtherCAT command */

   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

    command->type = EC_CMD_BWR;

   270

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

   271

    command->address.physical.mem = offset;

   272

    EC_FUNC_FOOTER;

   273

}

   274

   275

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

   276

   277

/**

   278

   Initializes an EtherCAT LRW command.

   279

   Logical read write.

   280

   \return 0 in case of success, else < 0

   281

*/

   282

   283

int ec_command_lrw(ec_command_t *command,

   284

                   /**< EtherCAT command */

   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

    command->type = EC_CMD_LRW;

   293

    command->address.logical = offset;

   294

    EC_FUNC_FOOTER;

   295

}

   296

   297

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