canmatc

#!/usr/bin/env python
#
# Copyright (c) 2008-2012, Georgia Tech Research Corporation
# All rights reserved.
#
# Author(s): Neil T. Dantam <ntd@gatech.edu>
# Georgia Tech Humanoid Robotics Lab
# Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#     * Redistributions of source code must retain the above
#       copyright notice, this list of conditions and the following
#       disclaimer.
#     * Redistributions in binary form must reproduce the above
#       copyright notice, this list of conditions and the following
#       disclaimer in the documentation and/or other materials
#       provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
# INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
# STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
# OF THE POSSIBILITY OF SUCH DAMAGE.


# FILE:        canmatc
# DESCRIPTION: Translate a CiA ESD file to C code
# AUTHOR:      Neil T. Dantam



# NOTE: The ConfigParser module has been renamed to configparser in
# Python 3

import ConfigParser
import sys
import re

from optparse import OptionParser

NS="CANMAT_"

def config2objdict( config, section ):
    d = {}
    for o in config.options(section):
        d[o] = config.get(section,o)
    return d

class EnumElt:
    "Entry in an enum"
    def __init__(self, name, value, desc ):
        self.name = name
        self.value = value
        self.desc = desc

def config2enumdict( config, section ):
    d = {}
    def ensure_param(name):
        if not name in d:
            d[name] = {}
        return d[name]
    # construct dict
    for o in config.options(section):
        if re.match(r'^[a-zA-Z0-9_]+\.value$', o):
            name = o.split(".")[0]
            ensure_param(name)["value"] = config.get(section,o)
        elif re.match(r'^[a-zA-Z0-9_]+\.description$', o):
            name = o.split(".")[0]
            ensure_param(name)["description"] = config.get(section,o)
    # sort
    e = []
    for name in d:
        val = 0;
        desc = name
        if "value" in d[name]:
            val = d[name]["value"]
        if "description" in d[name]:
            desc = d[name]["description"]
        e.append( EnumElt(name,val,desc) )

    def key(x):
        return x.value
    e.sort( key=key )
    return e

def is_section_index(section):
    return re.match(r'^[0-9a-fA-F]$', section)

def is_section_subindex(section):
    return re.match(r'^[0-9a-fA-F]+sub[0-9a-fA-F]+$', section)

def check_eds_entry(filename, section, params):
    if is_section_subindex( section ):
        # is var type
        if 'objecttype' in params:
            if 'VAR' != params['objecttype']:
                print "%s: section %s, bad ObjectType %s\n" % (filename, section, params['objecttype'])
                exit(-1)
        else:
            params['objecttype'] = 'VAR'
    return True

def parse_file( obj_dict, enum_dict, name ):
    '''Write all object dictaries sectons from file in obj_dict.
       Entries are indexed by EDS section.'''
    config = ConfigParser.ConfigParser()
    fp = open(name, "r")
    config.readfp( fp )
    for s in config.sections():
        if re.match(r'^[0-9a-fA-F]+(sub[0-9a-fA-F]+)?$', s):
            obj_dict[s] = config2objdict( config, s )
            check_eds_entry(name, s, obj_dict[s])
        elif re.match(r'^enum:[a-zA-Z_0-9]+$', s):
            enum_dict[s.split(":")[1]] = config2enumdict( config, s )

def datatype(s):
    s = s.upper();
    if ( re.match(r'BOOLEAN|((INTEGER|UNSIGNED)(8|16|32))|VISIBLE_STRING|TIME_OF_DAY|IDENTITY|DOMAIN|REAL32|REAL64', s ) ):
        return NS+"DATA_TYPE_"+s
    else: return s

def objecttype(s):
    s = s.upper();
    if( re.match(r'VAR|RECORD|ARRAY',s) ):
        return NS+"OBJECT_TYPE_"+s
    else: return s

def sect2index( section ):
    ''' Return (index,subindex)'''
    index = int( re.sub(r'sub.*', '', section), 16 )
    if re.match(r'^[0-9a-fA-F]+$', section):
        subindex = 0
    else:
        subindex = int( re.sub(r'^[0-9a-fA-F]*sub', '', section), 16 )
    return (index,subindex)

def map_enum( function, enum_dict, typename ):
    def h(elt):
        function(elt.name, elt.value, elt.desc)
    map( h, enum_dict[typename] )

def escape_const(s):
       s = re.sub(r' |-',"_", s.upper())
       s = re.sub(r'/',"_SUB_", s.upper())
       return s

def edsobj2cstruct( section, params, enum_dict ):
    '''Print and EDS section as C code'''
    (index,subindex) = sect2index( section )

    def try_param(param, cparam):
        if param in params:
            return '\t\t.%s=%s,\n' % (cparam, params[param])
        else:
            return ''

    ss = ['']
    def print_element( name, val, desc ):
        ss[0] += '\t\t\t{.name = "%s", .value=(%s), .description="%s"},\n' % (name, val, desc)
    def print_null():
        ss[0] += '\t\t\t{.name = NULL, .value=0, .description=NULL}\n'

    h = ''
    s =  '\t{\n'
    s += '\t\t.index=0x%04x,\n' % index
    s += '\t\t.subindex=0x%02x,\n' % subindex
    s += try_param('pdomapping', 'pdo_mapping')
    if 'datatype' in params:
        s += '\t\t.data_type=%s,\n' % datatype(params['datatype'])
    if 'objecttype' in params:
        s += '\t\t.object_type=%s,\n' % objecttype(params['objecttype'])
    if 'accesstype' in params:
        s += '\t\t.access_type=' + NS + 'ACCESS_%s,\n' % params['accesstype'].upper()
    if( 'maskenum' in params ) :
        map_enum( print_element, enum_dict, params['maskenum'] )
        print_null()
        s += '\t\t.mask_descriptor = (struct canmat_code_descriptor[]){\n%s\t\t},\n' % ss[0]
    if( 'valueenum' in params ) :
        map_enum( print_element, enum_dict, params['valueenum'] )
        print_null()
        s += '\t\t.value_descriptor = (struct canmat_code_descriptor[]){\n%s\t\t},\n' % ss[0]
    s += '\t\t.parameter_name="%s"\n' % params['parametername']
    s += '\t},\n'


    return (h,s)

def make_dict_header( name, namespace, params, i ):
    esc = escape_const(params['parametername'])
    defname = '%s_OBJ_%s' % (namespace.upper(), esc.upper())
    s = ( '#define %s (%s.obj + %d)\n' % (defname, name, i) )
    if( ('datatype' in params) and
        (re.match(r'((INTEGER|UNSIGNED)(8|16|32))', params['datatype'].upper()) ) and
        ('objecttype' in params) and
        ('VAR' == params['objecttype'].upper())
        ):
        t_short = params['datatype'][0].lower()
        t_short += re.sub(r'[A-Za-z]','', params['datatype'])
        # UL
        s += 'static inline canmat_status_t %s_ul_%s (\n' % (namespace, esc.lower())
        s += '\tstruct canmat_iface *cif, uint8_t node,\n'
        s += '\tCANMAT_%s *val, uint32_t *err)\n' % params['datatype'].upper()
        s += '{ return canmat_obj_ul_%s(cif, node, %s, val, err); }\n' % (t_short, defname)
        # DL
        s += 'static inline canmat_status_t %s_dl_%s (\n' % (namespace, esc.lower())
        s += '\tstruct canmat_iface *cif, uint8_t node,\n'
        s += '\tCANMAT_%s val, uint32_t *err)\n' % params['datatype'].upper()
        s += '{ return canmat_obj_dl_%s(cif, node, %s, val, err); }\n' % (t_short, defname)
    return s


def print_dict( name, output, header, namespace, odict, enum_dict ):
    # Sort by names
    def key_index(section):
        (index,subindex) = sect2index( section )
        return (index << 8) + subindex

    sections = odict.keys()
    sections.sort(key=key_index)

    def key_name(i):
        return odict[ sections[i] ]['parametername'].upper()

    btree_name = range(0,len(sections))
    btree_name.sort( key=key_name )

    # print
    s = ''
    h = ''
    h += "extern const canmat_dict_t %s;\n" % name
    s += "const canmat_dict_t %s = {\n" % name

    # length
    s += "\t.length=%d,\n" % len(sections)
    # name tree
    s += "\t.btree_name=(canmat_dict_name_tree_t[]){\n"
    for i in btree_name:
        s += "\t\t{.parameter_name=\"%s\",.i=%d},\n" % (odict[sections[i]]['parametername'], i)
    s += "\t},\n"

    # entries in index order
    s += "\t.obj=(canmat_obj_t[]){\n"
    i = 0
    for section in sections:
        (hp,sp) =  edsobj2cstruct( section, odict[section], enum_dict )
        h += hp
        s += sp
        h += make_dict_header( name, namespace, odict[section], i )
        i+=1
    s += "\t}\n"

    s += "};\n"

    output.write(s)
    header.write(h)


def print_enum( header, namespace, enum_dict ):
    def decl( s ):
        header.write(s)

    for typename in enum_dict.keys():
        decl( 'enum %s%s {\n' % (namespace, typename) )

        def print_element( elementname, val, desc ):
            decl( "\t %s%s_%s = %s, /* %s */\n" %
                  (namespace.upper(), typename.upper(), elementname.upper(), val, desc) )
        map_enum( print_element, enum_dict, typename )
        decl( "};\n" )



def main() :
    # Parse Arguments
    optparser = OptionParser()
    optparser.add_option("-o", "--output", dest="output", default="canmat_eds.c")
    optparser.add_option("-p", "--header", dest="header")
    optparser.add_option("-n", "--name", dest="name", default="canmat_dict")
    optparser.add_option("-N", "--namespace", dest="namespace", default="canmat_")
    optparser.add_option("-V", "--version",
                         action="store_true", dest="version")
    optparser.add_option("-x", "--exclude-dictionary",
                         action="store_true", dest="exclude_dictionary")
    optparser.add_option("-y", "--exclude-enum",
                         action="store_true", dest="exclude_enum")

    (opts,args) = optparser.parse_args()


    # Open output files
    if( not opts.exclude_dictionary ):
        output = open(opts.output, "w")

    if opts.header:
        header_name = opts.header
    else:
        header_name = re.sub(r'(\.(c|cc|cpp))?$',".h", opts.output)
    header = open(header_name,"w")

    # Read EDS file
    obj_dict = {}
    enum_dict = {}
    for eds in args:
        parse_file( obj_dict, enum_dict, eds )

    # Output
    if( not opts.exclude_dictionary ):
        s = ''
        s += '#include "socanmatic/eds.h"\n'
        #s += '#include "%s"\n' % header_name
        output.write(s)
        print_dict( opts.name, output, header, opts.namespace, obj_dict, enum_dict )

    if( not opts.exclude_enum ):
        print_enum( header, opts.namespace, enum_dict )

    #print enum_dict


main()