1

import os

     3

import subprocess

     4

import ctypes

     5

from threading import Thread

     6

import time

     7

import re

    10

from runtime.loglevels import LogLevelsDict

    11

    12

SDOAnswered = PLCBinary.SDOAnswered

    13

SDOAnswered.restype = None

    14

SDOAnswered.argtypes = []

    15

    16

SDOThread = None

    17

SDOProc = None

    18

Result = None

    19

    20

    21

def SDOThreadProc(*params):

    22

    global Result, SDOProc

    23

    if params[0] == "upload":

    24

        cmdfmt = "ethercat upload -p %d -t %s 0x%.4x 0x%.2x"

    25

    else:

    26

        cmdfmt = "ethercat download -p %d -t %s 0x%.4x 0x%.2x %s"

    27

    28

    command = cmdfmt % params[1:]

    29

    SDOProc = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)

    30

    res = SDOProc.wait()

    31

    output = SDOProc.communicate()[0]

    32

    33

    if params[0] == "upload":

    34

        Result = None

    35

        if res == 0:

    36

            if params[2] in ["float", "double"]:

    37

                Result = float(output)

    38

            elif params[2] in ["string", "octet_string", "unicode_string"]:

    39

                Result = output

    40

            else:

    41

                hex_value, dec_value = output.split()

    42

                if int(hex_value, 16) == int(dec_value):

    43

                    Result = int(dec_value)

    44

    else:

    45

        Result = res == 0

    46

    47

    SDOAnswered()

    48

    if res != 0:

    49

        PLCObject.LogMessage(

    50

            LogLevelsDict["WARNING"],

    51

            "%s : %s" % (command, output))

    52

    53

    54

def EthercatSDOUpload(pos, index, subindex, var_type):

    55

    global SDOThread

    56

    SDOThread = Thread(target=SDOThreadProc, args=["upload", pos, var_type, index, subindex])

    57

    SDOThread.start()

    58

    59

    60

def EthercatSDODownload(pos, index, subindex, var_type, value):

    61

    global SDOThread

    62

    SDOThread = Thread(target=SDOThreadProc, args=["download", pos, var_type, index, subindex, value])

    63

    SDOThread.start()

    64

    65

    66

def GetResult():

    67

    global Result

    68

    return Result

    69

    70

    71

KMSGPollThread = None

    72

StopKMSGThread = False

    73

    74

    75

def KMSGPollThreadProc():

    76

    """

    77

    Logs Kernel messages starting with EtherCAT

    78

    Uses GLibc wrapper to Linux syscall "klogctl"

    79

    Last 4 KB are polled, and lines compared to last

    80

    captured line to detect new lines

    81

    """

    82

    global StopKMSGThread

    83

    libc = ctypes.CDLL("libc.so.6")

    84

    klog = libc.klogctl

    85

    klog.argtypes = [ctypes.c_int, ctypes.c_char_p, ctypes.c_int]

    86

    klog.restype = ctypes.c_int

    87

    s = ctypes.create_string_buffer(4*1024)

    88

    last = None

    89

    while not StopKMSGThread:

    90

        bytes_to_read = klog(3, s, len(s)-1)

    91

        log = s.value[:bytes_to_read-1]

    92

        if last:

    93

            log = log.rpartition(last)[2]

    94

        if log:

    95

            last = log.rpartition('\n')[2]

    96

            for lvl, msg in re.findall(

    97

                            r'<(\d)>\[\s*\d*\.\d*\]\s*(EtherCAT\s*.*)$',

    98

                            log, re.MULTILINE):

    99

                PLCObject.LogMessage(

   100

                    LogLevelsDict[{

   101

                        "4": "WARNING",

   102

                        "3": "CRITICAL"}.get(lvl, "DEBUG")],

   103

                    msg)

   104

        time.sleep(0.5)

   105

   106

   107

def _runtime_etherlab_init():

   108

    global KMSGPollThread, StopKMSGThread

   109

    StopKMSGThread = False

   110

    KMSGPollThread = Thread(target=KMSGPollThreadProc)

   111

    KMSGPollThread.start()

   112

   113

   114

def _runtime_etherlab_cleanup():

   115

    global KMSGPollThread, StopKMSGThread, SDOProc, SDOThread

   116

    try:

   118

    except Exception:

   119

        pass

   120

    SDOThread = None

   121

    StopKMSGThread = True

   122

    KMSGPollThread = None