1

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

     2

 *

     3

 *  c o m m a n d . c

     4

 *

     5

 *  Methods of an EtherCAT command.

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include <linux/slab.h>

    12

#include <linux/delay.h>

    13

    14

#include "command.h"

    15

#include "master.h"

    16

    17

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

    18

    19

#define EC_FUNC_HEADER \

    20

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

    21

        return -1; \

    22

    command->index = 0; \

    23

    command->working_counter = 0; \

    24

    command->state = EC_CMD_INIT;

    25

    26

#define EC_FUNC_FOOTER \

    27

    command->data_size = data_size; \

    28

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

    29

    return 0;

    30

    31

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

    32

    33

/**

    34

   Command constructor.

    35

*/

    36

    37

void ec_command_init(ec_command_t *command)

    38

{

    39

    command->type = EC_CMD_NONE;

    40

    command->address.logical = 0x00000000;

    41

    command->data = NULL;

    42

    command->mem_size = 0;

    43

    command->data_size = 0;

    44

    command->index = 0x00;

    45

    command->working_counter = 0x00;

    46

    command->state = EC_CMD_INIT;

    47

}

    48

    49

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

    50

    51

/**

    52

   Command destructor.

    53

*/

    54

    55

void ec_command_clear(ec_command_t *command)

    56

{

    57

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

    58

}

    59

    60

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

    61

    62

/**

    63

   Allocates command data memory.

    64

   \return 0 in case of success, else < 0

    65

*/

    66

    67

int ec_command_prealloc(ec_command_t *command, size_t size)

    68

{

    69

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

    70

    71

    if (command->data) {

    72

        kfree(command->data);

    73

        command->data = NULL;

    74

        command->mem_size = 0;

    75

    }

    76

    77

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

    78

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

    79

        return -1;

    80

    }

    81

    82

    command->mem_size = size;

    83

    return 0;

    84

}

    85

    86

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

    87

    88

/**

    89

   Initializes an EtherCAT NPRD command.

    90

   Node-adressed physical read.

    91

   \return 0 in case of success, else < 0

    92

*/

    93

    94

int ec_command_nprd(ec_command_t *command,

    95

                    /**< EtherCAT command */

    96

                    uint16_t node_address,

    97

                    /**< configured station address */

    98

                    uint16_t offset,

    99

                    /**< physical memory address */

   100

                    size_t data_size

   101

                    /**< number of bytes to read */

   102

                    )

   103

{

   104

    if (unlikely(node_address == 0x0000))

   105

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

   106

   107

    EC_FUNC_HEADER;

   108

    command->type = EC_CMD_NPRD;

   109

    command->address.physical.slave = node_address;

   110

    command->address.physical.mem = offset;

   111

    EC_FUNC_FOOTER;

   112

}

   113

   114

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

   115

   116

/**

   117

   Initializes an EtherCAT NPWR command.

   118

   Node-adressed physical write.

   119

   \return 0 in case of success, else < 0

   120

*/

   121

   122

int ec_command_npwr(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 write */

   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_NPWR;

   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 APRD command.

   146

   Autoincrement physical read.

   147

   \return 0 in case of success, else < 0

   148

*/

   149

   150

int ec_command_aprd(ec_command_t *command,

   151

                    /**< EtherCAT command */

   152

                    uint16_t ring_position,

   153

                    /**< auto-increment position */

   154

                    uint16_t offset,

   155

                    /**< physical memory address */

   156

                    size_t data_size

   157

                    /**< number of bytes to read */

   158

                    )

   159

{

   160

    EC_FUNC_HEADER;

   161

    command->type = EC_CMD_APRD;

   162

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

   163

    command->address.physical.mem = offset;

   164

    EC_FUNC_FOOTER;

   165

}

   166

   167

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

   168

   169

/**

   170

   Initializes an EtherCAT APWR command.

   171

   Autoincrement physical write.

   172

   \return 0 in case of success, else < 0

   173

*/

   174

   175

int ec_command_apwr(ec_command_t *command,

   176

                    /**< EtherCAT command */

   177

                    uint16_t ring_position,

   178

                    /**< auto-increment position */

   179

                    uint16_t offset,

   180

                    /**< physical memory address */

   181

                    size_t data_size

   182

                    /**< number of bytes to write */

   183

                    )

   184

{

   185

    EC_FUNC_HEADER;

   186

    command->type = EC_CMD_APWR;

   187

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

   188

    command->address.physical.mem = offset;

   189

    EC_FUNC_FOOTER;

   190

}

   191

   192

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

   193

   194

/**

   195

   Initializes an EtherCAT BRD command.

   196

   Broadcast read.

   197

   \return 0 in case of success, else < 0

   198

*/

   199

   200

int ec_command_brd(ec_command_t *command,

   201

                   /**< EtherCAT command */

   202

                   uint16_t offset,

   203

                   /**< physical memory address */

   204

                   size_t data_size

   205

                   /**< number of bytes to read */

   206

                   )

   207

{

   208

    EC_FUNC_HEADER;

   209

    command->type = EC_CMD_BRD;

   210

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

   211

    command->address.physical.mem = offset;

   212

    EC_FUNC_FOOTER;

   213

}

   214

   215

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

   216

   217

/**

   218

   Initializes an EtherCAT BWR command.

   219

   Broadcast write.

   220

   \return 0 in case of success, else < 0

   221

*/

   222

   223

int ec_command_bwr(ec_command_t *command,

   224

                   /**< EtherCAT command */

   225

                   uint16_t offset,

   226

                   /**< physical memory address */

   227

                   size_t data_size

   228

                   /**< number of bytes to write */

   229

                   )

   230

{

   231

    EC_FUNC_HEADER;

   232

    command->type = EC_CMD_BWR;

   233

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

   234

    command->address.physical.mem = offset;

   235

    EC_FUNC_FOOTER;

   236

}

   237

   238

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

   239

   240

/**

   241

   Initializes an EtherCAT LRW command.

   242

   Logical read write.

   243

   \return 0 in case of success, else < 0

   244

*/

   245

   246

int ec_command_lrw(ec_command_t *command,

   247

                   /**< EtherCAT command */

   248

                   uint32_t offset,

   249

                   /**< logical address */

   250

                   size_t data_size

   251

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

   252

                   )

   253

{

   254

    EC_FUNC_HEADER;

   255

    command->type = EC_CMD_LRW;

   256

    command->address.logical = offset;

   257

    EC_FUNC_FOOTER;

   258

}

   259

   260

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