1

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

     2

 *

     3

 *  m a s t e r . c

     4

 *

     5

 *  Methoden für einen EtherCAT-Master.

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include <linux/module.h>

    12

#include <linux/kernel.h>

    13

#include <linux/string.h>

    14

#include <linux/slab.h>

    15

#include <linux/delay.h>

    16

    17

#include "../include/ecrt.h"

    18

#include "globals.h"

    19

#include "master.h"

    20

#include "slave.h"

    21

#include "types.h"

    22

#include "device.h"

    23

#include "command.h"

    24

    25

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

    26

    27

/**

    28

   Konstruktor des EtherCAT-Masters.

    29

*/

    30

    31

void ec_master_init(ec_master_t *master /**< EtherCAT-Master */)

    32

{

    33

    master->slaves = NULL;

    34

    master->device = NULL;

    35

    36

    INIT_LIST_HEAD(&master->commands);

    37

    INIT_LIST_HEAD(&master->domains);

    38

    39

    ec_master_reset(master);

    40

}

    41

    42

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

    43

    44

/**

    45

   Destruktor eines EtherCAT-Masters.

    46

    47

   Entfernt alle Kommandos aus der Liste, löscht den Zeiger

    48

   auf das Slave-Array und gibt die Prozessdaten frei.

    49

*/

    50

    51

void ec_master_clear(ec_master_t *master /**< EtherCAT-Master */)

    52

{

    53

    ec_master_reset(master);

    54

    55

    if (master->device) {

    56

        ec_device_clear(master->device);

    57

        kfree(master->device);

    58

    }

    59

}

    60

    61

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

    62

    63

/**

    64

   Setzt den Master zurück in den Ausgangszustand.

    65

    66

   Bei einem "release" sollte immer diese Funktion aufgerufen werden,

    67

   da sonst Slave-Liste, Domains, etc. weiter existieren.

    68

*/

    69

    70

void ec_master_reset(ec_master_t *master

    71

                     /**< Zeiger auf den zurückzusetzenden Master */

    72

                     )

    73

{

    74

    unsigned int i;

    75

    ec_command_t *command, *next_command;

    76

    ec_domain_t *domain, *next_domain;

    77

    78

    // Alle Slaves entfernen

    79

    if (master->slaves) {

    80

        for (i = 0; i < master->slave_count; i++) {

    81

            ec_slave_clear(master->slaves + i);

    82

        }

    83

        kfree(master->slaves);

    84

        master->slaves = NULL;

    85

    }

    86

    master->slave_count = 0;

    87

    88

    // Kommando-Warteschlange leeren

    89

    list_for_each_entry_safe(command, next_command, &master->commands, list) {

    90

        command->state = EC_CMD_ERROR;

    91

        list_del_init(&command->list);

    92

    }

    93

    94

    // Domain-Liste leeren

    95

    list_for_each_entry_safe(domain, next_domain, &master->domains, list) {

    96

        list_del(&domain->list);

    97

        ec_domain_clear(domain);

    98

        kfree(domain);

    99

    }

   100

   101

    master->command_index = 0;

   102

    master->debug_level = 0;

   103

    master->timeout = 100; // us

   104

   105

    master->slaves_responding = 0;

   106

    master->slave_states = EC_SLAVE_STATE_UNKNOWN;

   107

   108

    master->stats.timeouts = 0;

   109

    master->stats.delayed = 0;

   110

    master->stats.corrupted = 0;

   111

    master->stats.unmatched = 0;

   112

    master->stats.t_last = 0;

   113

}

   114

   115

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

   116

   117

/**

   118

   Öffnet das EtherCAT-Geraet des Masters.

   119

   120

   \return 0 wenn alles ok, < 0 wenn kein Gerät registriert wurde oder

   121

           es nicht geoeffnet werden konnte.

   122

*/

   123

   124

int ec_master_open(ec_master_t *master /**< Der EtherCAT-Master */)

   125

{

   126

    if (!master->device) {

   127

        EC_ERR("No device registered!\n");

   128

        return -1;

   129

    }

   130

   131

    if (ec_device_open(master->device)) {

   132

        EC_ERR("Could not open device!\n");

   133

        return -1;

   134

    }

   135

   136

    return 0;

   137

}

   138

   139

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

   140

   141

/**

   142

   Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet.

   143

*/

   144

   145

void ec_master_close(ec_master_t *master /**< EtherCAT-Master */)

   146

{

   147

    if (!master->device) {

   148

        EC_WARN("Warning - Trying to close an unregistered device!\n");

   149

        return;

   150

    }

   151

   152

    if (ec_device_close(master->device))

   153

        EC_WARN("Warning - Could not close device!\n");

   154

}

   155

   156

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

   157

   158

/**

   159

   Stellt ein Kommando in die Warteschlange.

   160

*/

   161

   162

void ec_master_queue_command(ec_master_t *master, /**< EtherCAT-Master */

   163

                             ec_command_t *command /**< Kommando */

   164

                             )

   165

{

   166

    ec_command_t *queued_command;

   167

   168

    // Ist das Kommando schon in der Warteschlange?

   169

    list_for_each_entry(queued_command, &master->commands, list) {

   170

        if (queued_command == command) {

   171

            command->state = EC_CMD_QUEUED;

   172

            if (unlikely(master->debug_level))

   173

                EC_WARN("command already queued.\n");

   174

            return;

   175

        }

   176

    }

   177

   178

    list_add_tail(&command->list, &master->commands);

   179

    command->state = EC_CMD_QUEUED;

   180

}

   181

   182

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

   183

   184

/**

   185

   Sendet die Kommandos in der Warteschlange.

   186

   187

   \return 0 bei Erfolg, sonst < 0

   188

*/

   189

   190

void ec_master_send_commands(ec_master_t *master /**< EtherCAT-Master */)

   191

{

   192

    ec_command_t *command;

   193

    size_t command_size;

   194

    uint8_t *frame_data, *cur_data;

   195

    void *follows_word;

   196

    cycles_t start = 0, end;

   197

   198

    if (unlikely(master->debug_level > 0)) {

   199

        EC_DBG("ec_master_send\n");

   200

        start = get_cycles();

   201

    }

   202

   203

    // Zeiger auf Socket-Buffer holen

   204

    frame_data = ec_device_tx_data(master->device);

   205

    cur_data = frame_data + EC_FRAME_HEADER_SIZE;

   206

    follows_word = NULL;

   207

   208

    // Aktuellen Frame mit Kommandos füllen

   209

    list_for_each_entry(command, &master->commands, list) {

   210

        if (command->state != EC_CMD_QUEUED) continue;

   211

   212

        // Passt das aktuelle Kommando noch in den aktuellen Rahmen?

   213

        command_size = EC_COMMAND_HEADER_SIZE + command->data_size

   214

            + EC_COMMAND_FOOTER_SIZE;

   215

        if (cur_data - frame_data + command_size > EC_MAX_FRAME_SIZE) break;

   216

   217

        command->state = EC_CMD_SENT;

   218

        command->index = master->command_index++;

   219

   220

        if (unlikely(master->debug_level > 0))

   221

            EC_DBG("adding command 0x%02X\n", command->index);

   222

   223

        // Command-Following-Flag im letzten Kommando setzen

   224

        if (follows_word)

   225

            EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);

   226

   227

        // EtherCAT command header

   228

        EC_WRITE_U8 (cur_data,     command->type);

   229

        EC_WRITE_U8 (cur_data + 1, command->index);

   230

        EC_WRITE_U32(cur_data + 2, command->address.logical);

   231

        EC_WRITE_U16(cur_data + 6, command->data_size & 0x7FF);

   232

        EC_WRITE_U16(cur_data + 8, 0x0000);

   233

        follows_word = cur_data + 6;

   234

        cur_data += EC_COMMAND_HEADER_SIZE;

   235

   236

        // EtherCAT command data

   237

        memcpy(cur_data, command->data, command->data_size);

   238

        cur_data += command->data_size;

   239

   240

        // EtherCAT command footer

   241

        EC_WRITE_U16(cur_data, command->working_counter);

   242

        cur_data += EC_COMMAND_FOOTER_SIZE;

   243

    }

   244

   245

    if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {

   246

        if (unlikely(master->debug_level > 0)) EC_DBG("nothing to send.\n");

   247

        return;

   248

    }

   249

   250

    // EtherCAT frame header

   251

    EC_WRITE_U16(frame_data, ((cur_data - frame_data

   252

                               - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);

   253

   254

    // Rahmen auffüllen

   255

    while (cur_data - frame_data < EC_MIN_FRAME_SIZE)

   256

        EC_WRITE_U8(cur_data++, 0x00);

   257

   258

    if (unlikely(master->debug_level > 0))

   259

        EC_DBG("Frame size: %i\n", cur_data - frame_data);

   260

   261

    // Send frame

   262

    ec_device_send(master->device, cur_data - frame_data);

   263

   264

    if (unlikely(master->debug_level > 0)) {

   265

        end = get_cycles();

   266

        EC_DBG("ec_master_send finished in %ius.\n",

   267

               (u32) (end - start) * 1000 / cpu_khz);

   268

    }

   269

}

   270

   271

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

   272

   273

/**

   274

   Wertet einen empfangenen Rahmen aus.

   275

   276

   \return 0 bei Erfolg, sonst < 0

   277

*/

   278

   279

void ec_master_receive(ec_master_t *master, /**< EtherCAT-Master */

   280

                      const uint8_t *frame_data, /**< Empfangene Daten */

   281

                      size_t size /**< Anzahl empfangene Datenbytes */

   282

                      )

   283

{

   284

    size_t frame_size, data_size;

   285

    uint8_t command_type, command_index;

   286

    unsigned int cmd_follows, matched;

   287

    const uint8_t *cur_data;

   288

    ec_command_t *command;

   289

   290

    if (unlikely(size < EC_FRAME_HEADER_SIZE)) {

   291

        master->stats.corrupted++;

   292

        ec_master_output_stats(master);

   293

        return;

   294

    }

   295

   296

    cur_data = frame_data;

   297

   298

    // Länge des gesamten Frames prüfen

   299

    frame_size = EC_READ_U16(cur_data) & 0x07FF;

   300

    cur_data += EC_FRAME_HEADER_SIZE;

   301

   302

    if (unlikely(frame_size > size)) {

   303

        master->stats.corrupted++;

   304

        ec_master_output_stats(master);

   305

        return;

   306

    }

   307

   308

    cmd_follows = 1;

   309

    while (cmd_follows) {

   310

        // Kommando-Header auswerten

   311

        command_type  = EC_READ_U8 (cur_data);

   312

        command_index = EC_READ_U8 (cur_data + 1);

   313

        data_size     = EC_READ_U16(cur_data + 6) & 0x07FF;

   314

        cmd_follows   = EC_READ_U16(cur_data + 6) & 0x8000;

   315

        cur_data += EC_COMMAND_HEADER_SIZE;

   316

   317

        if (unlikely(cur_data - frame_data

   318

                     + data_size + EC_COMMAND_FOOTER_SIZE > size)) {

   319

            master->stats.corrupted++;

   320

            ec_master_output_stats(master);

   321

            return;

   322

        }

   323

   324

        // Suche passendes Kommando in der Liste

   325

        matched = 0;

   326

        list_for_each_entry(command, &master->commands, list) {

   327

            if (command->state == EC_CMD_SENT

   328

                && command->type == command_type

   329

                && command->index == command_index

   330

                && command->data_size == data_size) {

   331

                matched = 1;

   332

                break;

   333

            }

   334

        }

   335

   336

        // Kein passendes Kommando in der Liste gefunden

   337

        if (!matched) {

   338

            master->stats.unmatched++;

   339

            ec_master_output_stats(master);

   340

            cur_data += data_size + EC_COMMAND_FOOTER_SIZE;

   341

            continue;

   342

        }

   343

   344

        // Empfangene Daten in Kommando-Datenspeicher kopieren

   345

        memcpy(command->data, cur_data, data_size);

   346

        cur_data += data_size;

   347

   348

        // Working-Counter setzen

   349

        command->working_counter = EC_READ_U16(cur_data);

   350

        cur_data += EC_COMMAND_FOOTER_SIZE;

   351

   352

        // Kommando aus der Liste entfernen

   353

        command->state = EC_CMD_RECEIVED;

   354

        list_del_init(&command->list);

   355

    }

   356

}

   357

   358

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

   359

   360

/**

   361

   Sendet ein einzelnes Kommando und wartet auf den Empfang.

   362

   363

   Wenn der Slave nicht antwortet, wird das Kommando

   364

   nochmals gesendet.

   365

   366

   \return 0 bei Erfolg, sonst < 0

   367

*/

   368

   369

int ec_master_simple_io(ec_master_t *master, /**< EtherCAT-Master */

   370

                        ec_command_t *command /**< Kommando */

   371

                        )

   372

{

   373

    unsigned int response_tries_left;

   374

   375

    response_tries_left = 10;

   376

   377

    while (1)

   378

    {

   379

        ec_master_queue_command(master, command);

   380

        ecrt_master_sync_io(master);

   381

   382

        if (command->state == EC_CMD_RECEIVED) {

   383

            if (likely(command->working_counter))

   384

                return 0;

   385

        }

   386

        else if (command->state == EC_CMD_TIMEOUT) {

   387

            EC_ERR("Simple-IO TIMEOUT!\n");

   388

            return -1;

   389

        }

   390

        else if (command->state == EC_CMD_ERROR) {

   391

            EC_ERR("Simple-IO command error!\n");

   392

            return -1;

   393

        }

   394

   395

        // Keine direkte Antwort. Dem Slave Zeit lassen...

   396

        udelay(100);

   397

   398

        if (unlikely(--response_tries_left)) {

   399

            EC_ERR("No response in simple-IO!\n");

   400

            return -1;

   401

        }

   402

    }

   403

}

   404

   405

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

   406

   407

/**

   408

   Durchsucht den EtherCAT-Bus nach Slaves.

   409

   410

   Erstellt ein Array mit allen Slave-Informationen die für den

   411

   weiteren Betrieb notwendig sind.

   412

   413

   \return 0 bei Erfolg, sonst < 0

   414

*/

   415

   416

int ec_master_bus_scan(ec_master_t *master /**< EtherCAT-Master */)

   417

{

   418

    ec_command_t command;

   419

    ec_slave_t *slave;

   420

    ec_slave_ident_t *ident;

   421

    unsigned int i;

   422

    uint8_t data[2];

   423

   424

    if (master->slaves || master->slave_count) {

   425

        EC_ERR("Slave scan already done!\n");

   426

        return -1;

   427

    }

   428

   429

    // Determine number of slaves on bus

   430

    ec_command_init_brd(&command, 0x0000, 4);

   431

    if (unlikely(ec_master_simple_io(master, &command))) return -1;

   432

    master->slave_count = command.working_counter;

   433

    EC_INFO("Found %i slaves on bus.\n", master->slave_count);

   434

   435

    if (!master->slave_count) return 0;

   436

   437

    if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count

   438

                                                  * sizeof(ec_slave_t),

   439

                                                  GFP_KERNEL))) {

   440

        EC_ERR("Could not allocate memory for slaves!\n");

   441

        return -1;

   442

    }

   443

   444

    // Init slaves

   445

    for (i = 0; i < master->slave_count; i++) {

   446

        slave = master->slaves + i;

   447

        ec_slave_init(slave, master);

   448

        slave->ring_position = i;

   449

        slave->station_address = i + 1;

   450

    }

   451

   452

    // For every slave in the list

   453

    for (i = 0; i < master->slave_count; i++)

   454

    {

   455

        slave = master->slaves + i;

   456

   457

        // Write station address

   458

        EC_WRITE_U16(data, slave->station_address);

   459

        ec_command_init_apwr(&command, slave->ring_position,

   460

                             0x0010, sizeof(uint16_t), data);

   461

        if (unlikely(ec_master_simple_io(master, &command))) {

   462

            EC_ERR("Writing station address failed on slave %i!\n", i);

   463

            return -1;

   464

        }

   465

   466

        // Fetch all slave information

   467

        if (ec_slave_fetch(slave)) return -1;

   468

   469

        // Search for identification in "database"

   470

        ident = slave_idents;

   471

        while (ident->type) {

   472

            if (unlikely(ident->vendor_id == slave->sii_vendor_id

   473

                         && ident->product_code == slave->sii_product_code)) {

   474

                slave->type = ident->type;

   475

                break;

   476

            }

   477

            ident++;

   478

        }

   479

   480

        if (!slave->type)

   481

            EC_WARN("Unknown slave device (vendor 0x%08X, code 0x%08X) at"

   482

                    " position %i.\n", slave->sii_vendor_id,

   483

                    slave->sii_product_code, i);

   484

    }

   485

   486

    return 0;

   487

}

   488

   489

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

   490

   491

/**

   492