andrej@2405: from __future__ import absolute_import
andrej@2357: import os
andrej@2404: import signal
andrej@2357: import subprocess
andrej@2357: import ctypes
Laurent@2086: from threading import Thread
andrej@2357: import time
andrej@2357: import re
andrej@2404: 
andrej@2404: import runtime.PLCObject as PLCObject
andrej@2401: from runtime.loglevels 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: 
andrej@2360: 
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()
andrej@2375:     if res != 0:
Edouard@2115:         PLCObject.LogMessage(
andrej@2355:             LogLevelsDict["WARNING"],
andrej@2363:             "%s : %s" % (command, output))
andrej@2355: 
andrej@2360: 
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: 
andrej@2360: 
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: 
andrej@2360: 
Laurent@2086: def GetResult():
Laurent@2086:     return Result
Edouard@2114: 
andrej@2370: 
andrej@2365: KMSGPollThread = None
andrej@2365: StopKMSGThread = False
andrej@2360: 
andrej@2360: 
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:     """
andrej@2365:     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
andrej@2365:     s = ctypes.create_string_buffer(4*1024)
Edouard@2114:     last = None
Edouard@2114:     while not StopKMSGThread:
andrej@2372:         bytes_to_read = klog(3, s, len(s)-1)
andrej@2372:         log = s.value[:bytes_to_read-1]
andrej@2375:         if last:
Edouard@2114:             log = log.rpartition(last)[2]
andrej@2375:         if log:
Edouard@2114:             last = log.rpartition('\n')[2]
andrej@2363:             for lvl, msg in re.findall(
andrej@2407:                     r'<(\d)>\[\s*\d*\.\d*\]\s*(EtherCAT\s*.*)$',
andrej@2407:                     log, re.MULTILINE):
Edouard@2116:                 PLCObject.LogMessage(
Edouard@2116:                     LogLevelsDict[{
andrej@2363:                         "4": "WARNING",
andrej@2363:                         "3": "CRITICAL"}.get(lvl, "DEBUG")],
Edouard@2116:                     msg)
andrej@2355:         time.sleep(0.5)
Edouard@2114: 
andrej@2360: 
Edouard@2114: def _runtime_etherlab_init():
Edouard@2114:     global KMSGPollThread, StopKMSGThread
Edouard@2114:     StopKMSGThread = False
andrej@2366:     KMSGPollThread = Thread(target=KMSGPollThreadProc)
Edouard@2114:     KMSGPollThread.start()
Edouard@2114: 
andrej@2360: 
Edouard@2114: def _runtime_etherlab_cleanup():
andrej@2422:     global KMSGPollThread, StopKMSGThread, SDOThread
Laurent@2132:     try:
andrej@2404:         os.kill(SDOProc.pid, signal.SIGTERM)
andrej@2353:     except Exception:
Laurent@2132:         pass
Laurent@2132:     SDOThread = None
Edouard@2114:     StopKMSGThread = True
Edouard@2114:     KMSGPollThread = None