Simple Modbus request builder with va_arg (maybe we need this in liblightmodbus?)

modrq.c

//TODO polish this (and tidy!)
//Send Modbus request and parses response
int modrq( uint8_t priority, uint8_t function, uint8_t addr, uint16_t reg, uint16_t cnt, ...  )
{
	va_list ap;

	uint16_t andmask, ormask, regtype;
	uint8_t waccess;
	int ec = 1;

	//TODO this is bad!!!!
	//Determine data type
	switch ( function )
	{
		case 02:
			regtype = MODBUS_DISCRETE_INPUT;
			break;

		case 04:
			regtype = MODBUS_INPUT_REGISTER;
			break;

		case 01:
		case 05:
		case 15:
			regtype = MODBUS_COIL;
			break;

		case 03:
		case 06:
		case 16:
		case 22:
			regtype = MODBUS_HOLDING_REGISTER;
			break;

		default:
			return MODBUS_BAD_FUNCTION;
			break;

	}

	//Does the request need read or write access?
	waccess = ( memchr( "\05\15\06\16\22", function, 5 ) != NULL );

	//Check access rights
	if ( modacc( priority, regtype, addr, waccess, reg, cnt ) )
		return MODBUS_UNPERMITTED;

	//By this time, unpermitted operations should have been discarded - build the request
	va_start( ap, cnt );
	switch ( function )
	{
		case 01:
		case 02:
			ec = modbusBuildRequest0102( &sys.modbus.master, function, addr, reg, cnt );
			break;

		case 03:
		case 04:
			ec = modbusBuildRequest0304( &sys.modbus.master, function, addr, reg, cnt );
			break;

		case 05:
			ec = modbusBuildRequest05( &sys.modbus.master, addr, reg, va_arg( ap, int ) );
			break;

		case 06:
			ec = modbusBuildRequest06( &sys.modbus.master, addr, reg, va_arg( ap, int ) );
			break;

		case 15:
			ec = modbusBuildRequest15( &sys.modbus.master, addr, reg, cnt, va_arg( ap, uint8_t* ) );
			break;

		case 16:
			ec = modbusBuildRequest16( &sys.modbus.master, addr, reg, cnt, va_arg( ap, uint16_t* ) );
			break;

		case 22:
			andmask = va_arg( ap, int );
			ormask = va_arg( ap, int );
			ec = modbusBuildRequest22( &sys.modbus.master, addr, reg, andmask, ormask );
			break;

		default:
			return MODBUS_BAD_FUNCTION;
			break;
	}
	va_end( ap );

	//Build failed
	if ( ec != MODBUS_ERROR_OK ) return ec;

	//Final stage - do the actual request
	modtx( sys.modbus.master.request.frame, sys.modbus.master.request.length );
	modcl( );

	//Wait
	timeout( TURNAROUND_TIMEOUT );
	while ( !sys.modbus.lock && !TIMEOUT );
	timeout( TIMEOUT_ABORT );

	//Copy data from USART buffer
	memcpy( sys.modbus.master.response.frame, sys.modbus.data, sys.modbus.cnt );
	sys.modbus.master.response.length = sys.modbus.cnt;

	//Parse response
	return modbusParseResponse( &sys.modbus.master );
}