andrej@2357: import os
andrej@2357: import subprocess
andrej@2357: import sys
andrej@2357: import ctypes
Laurent@2086: from threading import Thread
andrej@2357: import ctypes
andrej@2357: import time
andrej@2357: import re
Edouard@2115: from targets.typemapping import LogLevelsDict
Laurent@2086: 
Laurent@2086: SDOAnswered = PLCBinary.SDOAnswered
Laurent@2086: SDOAnswered.restype = None
Laurent@2086: SDOAnswered.argtypes = []
Laurent@2086: 
Laurent@2086: SDOThread = None
Laurent@2132: SDOProc = None
Laurent@2086: Result = None
Laurent@2086: 
Laurent@2086: def SDOThreadProc(*params):
Laurent@2132:     global Result, SDOProc
Laurent@2086:     if params[0] == "upload":
Edouard@2115:         cmdfmt = "ethercat upload -p %d -t %s 0x%.4x 0x%.2x"
Laurent@2086:     else:
Edouard@2115:         cmdfmt = "ethercat download -p %d -t %s 0x%.4x 0x%.2x %s"
andrej@2355: 
Edouard@2115:     command = cmdfmt % params[1:]
Laurent@2132:     SDOProc = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
Laurent@2132:     res = SDOProc.wait()
Laurent@2132:     output = SDOProc.communicate()[0]
andrej@2355: 
Laurent@2086:     if params[0] == "upload":
Laurent@2086:         Result = None
Laurent@2086:         if res == 0:
Laurent@2086:             if params[2] in ["float", "double"]:
Laurent@2086:                 Result = float(output)
Laurent@2086:             elif params[2] in ["string", "octet_string", "unicode_string"]:
Laurent@2086:                 Result = output
Laurent@2086:             else:
Laurent@2086:                 hex_value, dec_value = output.split()
Laurent@2086:                 if int(hex_value, 16) == int(dec_value):
Laurent@2086:                     Result = int(dec_value)
Laurent@2086:     else:
Laurent@2086:         Result = res == 0
andrej@2355: 
Laurent@2086:     SDOAnswered()
Edouard@2115:     if res != 0 :
Edouard@2115:         PLCObject.LogMessage(
andrej@2355:             LogLevelsDict["WARNING"],
Edouard@2115:             "%s : %s"%(command,output))
andrej@2355: 
Laurent@2086: def EthercatSDOUpload(pos, index, subindex, var_type):
Laurent@2086:     global SDOThread
Laurent@2086:     SDOThread = Thread(target=SDOThreadProc, args=["upload", pos, var_type, index, subindex])
Laurent@2086:     SDOThread.start()
andrej@2355: 
Laurent@2086: def EthercatSDODownload(pos, index, subindex, var_type, value):
Laurent@2086:     global SDOThread
Laurent@2086:     SDOThread = Thread(target=SDOThreadProc, args=["download", pos, var_type, index, subindex, value])
Laurent@2086:     SDOThread.start()
Laurent@2086: 
Laurent@2086: def GetResult():
Laurent@2086:     global Result
Laurent@2086:     return Result
Edouard@2114: 
Edouard@2114: KMSGPollThread=None
Edouard@2114: StopKMSGThread=False
Edouard@2114: def KMSGPollThreadProc():
Edouard@2114:     """
Edouard@2114:     Logs Kernel messages starting with EtherCAT
Edouard@2114:     Uses GLibc wrapper to Linux syscall "klogctl"
andrej@2355:     Last 4 KB are polled, and lines compared to last
Edouard@2114:     captured line to detect new lines
Edouard@2114:     """
Edouard@2114:     global StopKMSGThread
Edouard@2114:     libc=ctypes.CDLL("libc.so.6")
Edouard@2114:     klog = libc.klogctl
Edouard@2114:     klog.argtypes = [ctypes.c_int, ctypes.c_char_p, ctypes.c_int]
Edouard@2114:     klog.restype = ctypes.c_int
Edouard@2114:     s=ctypes.create_string_buffer(4*1024)
Edouard@2114:     last = None
Edouard@2114:     while not StopKMSGThread:
Edouard@2114:         l = klog(3,s,len(s)-1)
Edouard@2114:         log = s.value[:l-1]
Edouard@2114:         if last :
Edouard@2114:             log = log.rpartition(last)[2]
andrej@2355:         if log :
Edouard@2114:             last = log.rpartition('\n')[2]
Edouard@2116:             for lvl,msg in re.findall(
Edouard@2116:                             r'<(\d)>\[\s*\d*\.\d*\]\s*(EtherCAT\s*.*)$',
Edouard@2116:                             log, re.MULTILINE):
Edouard@2116:                 PLCObject.LogMessage(
Edouard@2116:                     LogLevelsDict[{
Edouard@2116:                         "4":"WARNING",
Edouard@2116:                         "3":"CRITICAL"}.get(lvl,"DEBUG")],
Edouard@2116:                     msg)
andrej@2355:         time.sleep(0.5)
Edouard@2114: 
Edouard@2114: def _runtime_etherlab_init():
Edouard@2114:     global KMSGPollThread, StopKMSGThread
Edouard@2114:     StopKMSGThread = False
Edouard@2114:     KMSGPollThread = Thread(target = KMSGPollThreadProc)
Edouard@2114:     KMSGPollThread.start()
Edouard@2114: 
Edouard@2114: def _runtime_etherlab_cleanup():
Laurent@2132:     global KMSGPollThread, StopKMSGThread, SDOProc, SDOThread
Laurent@2132:     try:
Laurent@2132:         os.kill(SDOProc.pid, SIGTERM)
andrej@2353:     except Exception:
Laurent@2132:         pass
Laurent@2132:     SDOThread = None
Edouard@2114:     StopKMSGThread = True
Edouard@2114:     KMSGPollThread = None