]> git.tdb.fi Git - model-railway-devices.git/commitdiff
Add support for Märklin MFX protocol in arducontrol
authorMikko Rasa <tdb@tdb.fi>
Sun, 3 Nov 2013 17:41:15 +0000 (19:41 +0200)
committerMikko Rasa <tdb@tdb.fi>
Sun, 3 Nov 2013 17:43:24 +0000 (19:43 +0200)
It hasn't been exhaustively tested yet, but decoder registration and
function control at least works.

arducontrol/Makefile
arducontrol/commands.h
arducontrol/interface.c
arducontrol/mfx.c [new file with mode: 0644]
arducontrol/mfx.h [new file with mode: 0644]

index be2ca328bc6e75a8eadbffbab839d1ffa8219aed..73dfb0f35227fac0aa9ffc885c33252d347d8bf3 100644 (file)
@@ -1,4 +1,4 @@
 FEATURES := SERIAL_ASYNC
 include ../common/build.mk
 
-arducontrol.elf: adc.o interface.o monitor.o motorola.o output.o serial.o s88.o timer.o
+arducontrol.elf: adc.o interface.o mfx.o monitor.o motorola.o output.o serial.o s88.o timer.o
index 7e2370a2d8d94abb818803f4d8d000a299221b02..1fd027c112f6159eb5531cea80913933874ea8d5 100644 (file)
@@ -14,6 +14,14 @@ enum Command
        MOTOROLA_SPEED_DIRECTION = 0x13,
        MOTOROLA_SPEED_FUNCTION = 0x14,
        MOTOROLA_SOLENOID = 0x15,
+       MFX_SET_STATION_ID = 0x21,
+       MFX_ANNOUNCE = 0x22,
+       MFX_SEARCH = 0x23,
+       MFX_ASSIGN_ADDRESS = 0x24,
+       MFX_PING = 0x25,
+       MFX_SPEED = 0x28,
+       MFX_SPEED_FUNCS8 = 0x29,
+       MFX_SPEED_FUNCS16 = 0x2A,
        S88_READ = 0x30,
        COMMAND_OK = 0x80,
        RECEIVE_OVERRUN = 0x81,
@@ -25,7 +33,8 @@ enum Command
        TRACK_CURRENT = 0xC0,
        INPUT_VOLTAGE = 0xC1,
        POWER_STATE = 0xC2,
-       S88_DATA = 0xD0
+       S88_DATA = 0xD0,
+       MFX_FEEDBACK = 0xD1
 };
 
 #endif
index 014e8e98bcaa0ac5eacbeb061f1a38754e1a2ae5..63b1ebd1700b0c11d1e193ed1b0c1c3ef4eb0a91 100644 (file)
@@ -1,5 +1,6 @@
 #include <avr/io.h>
 #include "interface.h"
+#include "mfx.h"
 #include "monitor.h"
 #include "motorola.h"
 #include "output.h"
@@ -75,6 +76,8 @@ static uint8_t dispatch_command(const uint8_t *cmd, uint8_t length)
        }
        else if(type==1)
                return motorola_command(cmd, length);
+       else if(type==2)
+               return mfx_command(cmd, length);
        else if(type==3)
                return s88_command(cmd, length);
        else
diff --git a/arducontrol/mfx.c b/arducontrol/mfx.c
new file mode 100644 (file)
index 0000000..a39ce1f
--- /dev/null
@@ -0,0 +1,441 @@
+#include "interface.h"
+#include "mfx.h"
+#include "monitor.h"
+#include "output.h"
+
+typedef struct
+{
+       uint8_t ones_count;
+       uint8_t crc8;
+} MfxEncodingState;
+
+static uint32_t station_id = 0x12345678;
+static uint8_t feedback_threshold = 30;
+
+void mfx_set_station_id(uint32_t id)
+{
+       station_id = id;
+}
+
+static OutputPacket *mfx_create_packet(MfxEncodingState *state)
+{
+       OutputPacket *packet = output_create_packet();
+       packet->bit_duration = 4;
+       packet->repeat_count = 1;
+       packet->final_delay = 0;
+       packet->length = 10;
+       packet->data[0] = 0x9B;
+       packet->data[1] = 0;
+       state->ones_count = 0;
+       state->crc8 = 0x7A;
+       return packet;
+}
+
+static inline void mfx_update_crc8(MfxEncodingState *state, uint8_t bits, uint8_t length)
+{
+       for(uint8_t i=length; i--; )
+       {
+               uint8_t bit = (bits>>i)&1;
+               if(bit^(state->crc8>>7))
+                       state->crc8 = (state->crc8<<1)^7;
+               else
+                       state->crc8 <<= 1;
+       }
+}
+
+static inline uint8_t mfx_get_output_level(const OutputPacket *packet)
+{
+       uint8_t b = packet->length-1;
+       return (packet->data[b>>3]>>(b&7))&1;
+}
+
+static inline void mfx_ensure_low_level(OutputPacket *packet)
+{
+       if(mfx_get_output_level(packet))
+       {
+               // Motorola decoders require a low logic level when idle.
+               uint8_t out_bit = packet->length;
+               if((out_bit&7)==0)
+                       packet->data[out_bit>>3] = 0;
+               ++packet->length;
+       }
+}
+
+static inline void mfx_encode_flag_pairs(OutputPacket *packet, uint8_t count)
+{
+       static const uint8_t flag_bits[5] = { 0x9B, 0x6C, 0xB2, 0xC9, 0x26 };
+
+       uint8_t invert = mfx_get_output_level(packet)*0xFF;
+       uint8_t shift = packet->length&7;
+       uint8_t total_bits = count*10;
+       uint8_t tail_bits = (total_bits+shift)&7;
+       uint8_t out_byte = packet->length>>3;
+       if(shift)
+       {
+               uint8_t head_bits = 8-shift;
+               packet->data[out_byte++] |= (flag_bits[0]^invert)<<shift;
+
+               uint8_t i = 1;
+               uint8_t bytes = (total_bits-head_bits)>>3;
+               uint8_t carry = flag_bits[0]>>head_bits;
+               while(bytes--)
+               {
+                       packet->data[out_byte++] = (carry|(flag_bits[i]<<shift))^invert;
+                       carry = flag_bits[i]>>head_bits;
+
+                       if(++i>=sizeof(flag_bits))
+                               i -= sizeof(flag_bits);
+               }
+
+               if(tail_bits>0)
+                       packet->data[out_byte] = ((carry|(flag_bits[i]<<shift))^invert)&((1<<tail_bits)-1);
+       }
+       else
+       {
+               uint8_t i = 0;
+               uint8_t bytes = total_bits>>3;
+               while(bytes--)
+               {
+                       packet->data[out_byte++] = flag_bits[i]^invert;
+
+                       if(++i>=sizeof(flag_bits))
+                               i -= sizeof(flag_bits);
+               }
+
+               uint8_t tail_bits = (total_bits+shift)&7;
+               if(tail_bits>0)
+                       packet->data[out_byte] = (flag_bits[i]^invert)&((1<<tail_bits)-1);
+       }
+
+       packet->length += total_bits;
+}
+
+static inline void mfx_encode_bits8_raw(OutputPacket *packet, MfxEncodingState *state, uint8_t bits, uint8_t length)
+{
+       uint8_t out_bit = packet->length;
+       uint8_t out_level = mfx_get_output_level(packet);
+
+       for(uint8_t i=length; i--; )
+       {
+               uint8_t bit = (bits>>i)&1;
+
+               if(state->ones_count>=8)
+               {
+                       ++i;
+                       bit = 0;
+                       state->ones_count = 0;
+               }
+               else if(bit)
+                       ++state->ones_count;
+               else
+                       state->ones_count = 0;
+
+               if(out_level==bit)
+                       bit ^= 3;
+
+               // out_bit is always even at this point
+               if((out_bit&7)==0)
+                       packet->data[out_bit>>3] = bit;
+               else
+                       packet->data[out_bit>>3] |= bit<<(out_bit&7);
+               out_bit += 2;
+               out_level = bit>>1;
+       }
+
+       packet->length = out_bit;
+}
+
+static inline void mfx_encode_bits8(OutputPacket *packet, MfxEncodingState *state, uint8_t bits, uint8_t length)
+{
+       mfx_encode_bits8_raw(packet, state, bits, length);
+       mfx_update_crc8(state, bits, length);
+}
+
+static inline void mfx_encode_bits16(OutputPacket *packet, MfxEncodingState *state, uint16_t bits, uint8_t length)
+{
+       mfx_encode_bits8(packet, state, bits>>8, length-8);
+       mfx_encode_bits8(packet, state, bits, 8);
+}
+
+static inline void mfx_encode_bits32(OutputPacket *packet, MfxEncodingState *state, uint32_t bits, uint8_t length)
+{
+       if(length>24)
+       {
+               mfx_encode_bits8(packet, state, bits>>24, length-24);
+               mfx_encode_bits8(packet, state, bits>>16, 8);
+       }
+       else
+               mfx_encode_bits8(packet, state, bits>>16, length-16);
+       mfx_encode_bits8(packet, state, bits>>8, 8);
+       mfx_encode_bits8(packet, state, bits, 8);
+}
+
+static inline void mfx_address_field(OutputPacket *packet, MfxEncodingState *state, uint16_t address)
+{
+       if(address<0x80)
+               mfx_encode_bits16(packet, state, 0x100|address, 9);
+       else if(address<0x200)
+               mfx_encode_bits16(packet, state, 0xC00|address, 12);
+       else if(address<0x800)
+               mfx_encode_bits16(packet, state, 0x7000|address, 15);
+       else
+       {
+               mfx_encode_bits8(packet, state, 0xF, 4);
+               mfx_encode_bits16(packet, state, address, 14);
+       }
+}
+
+void mfx_speed_field(OutputPacket *packet, MfxEncodingState *state, uint8_t speed_dir)
+{
+       if(speed_dir&0x0F)
+       {
+               // Field type: 001
+               mfx_encode_bits8(packet, state, 0x1, 3);
+               mfx_encode_bits8(packet, state, speed_dir, 8);
+       }
+       else
+               // Field type: 000
+               mfx_encode_bits8(packet, state, speed_dir>>4, 7);
+}
+
+static void mfx_finish_packet(OutputPacket *packet, MfxEncodingState *state)
+{
+       mfx_encode_bits8_raw(packet, state, state->crc8, 8);
+       mfx_encode_flag_pairs(packet, 2);
+       mfx_ensure_low_level(packet);
+}
+
+static void mfx_finish_packet_feedback(OutputPacket *packet, MfxEncodingState *state)
+{
+       mfx_encode_bits8_raw(packet, state, state->crc8, 8);
+       mfx_encode_flag_pairs(packet, 11);
+       mfx_encode_bits8_raw(packet, state, 0x3, 4);
+       uint8_t fill = (1-mfx_get_output_level(packet))*0xFF;
+
+       for(uint8_t i=0; i<2; ++i)
+       {
+               packet = output_create_chained_packet();
+               packet->bit_duration = 4;
+               packet->repeat_count = 1;
+               packet->final_delay = 0;
+               packet->trigger_position = 112;
+               packet->trigger_value = i+1;
+
+               for(uint8_t j=0; j<16; ++j)
+                       packet->data[j] = fill;
+               packet->length = 128;
+               mfx_encode_flag_pairs(packet, 1+i);
+               fill ^= 0xFF;
+       }
+
+       mfx_ensure_low_level(packet);
+}
+
+static void mfx_receive_feedback()
+{
+       /* The decoder should activate a 52.6 kHz carrier to indicate positive
+       acknowledgement, but so far I've been unable to build a circuit that detects
+       it.  The increased current draw can nevertheless be measured. */
+       uint8_t state = 0;
+       uint16_t current[3];
+       current[0] = monitor_track_current();
+       while(state<2)
+       {
+               uint8_t trig = output_get_trigger();
+               if(trig!=state)
+               {
+                       current[trig] = monitor_track_current();
+                       state = trig;
+               }
+               monitor_check();
+       }
+
+       uint8_t reply[2];
+       reply[0] = MFX_FEEDBACK;
+       if(current[1]>feedback_threshold)
+       {
+               current[1] -= feedback_threshold;
+               reply[1] = (current[1]>current[0] && current[1]>current[2]);
+       }
+       else
+               reply[1] = 0;
+       interface_send(reply, 2);
+}
+
+void mfx_announce_packet(uint16_t serial)
+{
+       MfxEncodingState state;
+       OutputPacket *packet = mfx_create_packet(&state);
+       mfx_address_field(packet, &state, 0);
+       // Packet type: 111101
+       mfx_encode_bits8(packet, &state, 0x3D, 6);
+       mfx_encode_bits32(packet, &state, station_id, 32);
+       mfx_encode_bits16(packet, &state, serial, 16);
+       mfx_finish_packet(packet, &state);
+       output_send_packet();
+}
+
+void mfx_search_packet(uint32_t mask_bits, uint8_t mask_size)
+{
+       MfxEncodingState state;
+       OutputPacket *packet = mfx_create_packet(&state);
+       mfx_address_field(packet, &state, 0);
+       // Packet type: 111010
+       mfx_encode_bits8(packet, &state, 0x3A, 6);
+       mfx_encode_bits8(packet, &state, mask_size, 6);
+       mfx_encode_bits32(packet, &state, mask_bits, 32);
+       mfx_finish_packet_feedback(packet, &state);
+       output_send_packet();
+}
+
+void mfx_assign_address_packet(uint16_t addr, uint32_t id)
+{
+       MfxEncodingState state;
+       OutputPacket *packet = mfx_create_packet(&state);
+       mfx_address_field(packet, &state, 0);
+       // Packet type: 111011
+       mfx_encode_bits8(packet, &state, 0x3B, 6);
+       mfx_encode_bits16(packet, &state, addr, 14);
+       mfx_encode_bits32(packet, &state, id, 32);
+       mfx_finish_packet(packet, &state);
+       output_send_packet();
+}
+
+void mfx_ping_packet(uint16_t addr, uint32_t id)
+{
+       MfxEncodingState state;
+       OutputPacket *packet = mfx_create_packet(&state);
+       mfx_address_field(packet, &state, addr);
+       // Packet type: 111100
+       mfx_encode_bits8(packet, &state, 0x3C, 6);
+       mfx_encode_bits32(packet, &state, id, 32);
+       mfx_finish_packet_feedback(packet, &state);
+       output_send_packet();
+}
+
+void mfx_speed_packet(uint16_t addr, uint8_t speed_dir)
+{
+       MfxEncodingState state;
+       OutputPacket *packet = mfx_create_packet(&state);
+       mfx_address_field(packet, &state, addr);
+       mfx_speed_field(packet, &state, speed_dir);
+       mfx_finish_packet(packet, &state);
+       output_send_packet();
+}
+
+void mfx_speed_funcs8_packet(uint16_t addr, uint8_t speed_dir, uint8_t funcs)
+{
+       MfxEncodingState state;
+       OutputPacket *packet = mfx_create_packet(&state);
+       mfx_address_field(packet, &state, addr);
+       mfx_speed_field(packet, &state, speed_dir);
+       if(funcs&0xF0)
+       {
+               // Field type: 0110
+               mfx_encode_bits8(packet, &state, 0x6, 4);
+               mfx_encode_bits8(packet, &state, funcs, 8);
+       }
+       else
+               // Field type: 010
+               mfx_encode_bits8(packet, &state, 0x20|funcs, 7);
+       mfx_finish_packet(packet, &state);
+       output_send_packet();
+}
+
+void mfx_speed_funcs16_packet(uint16_t addr, uint8_t speed_dir, uint16_t funcs)
+{
+       MfxEncodingState state;
+       OutputPacket *packet = mfx_create_packet(&state);
+       mfx_address_field(packet, &state, addr);
+       mfx_speed_field(packet, &state, speed_dir);
+       // Field type: 0111
+       mfx_encode_bits8(packet, &state, 0x7, 4);
+       mfx_encode_bits16(packet, &state, funcs, 16);
+       mfx_finish_packet(packet, &state);
+       output_send_packet();
+}
+
+uint8_t mfx_command(const uint8_t *cmd, uint8_t length)
+{
+       if(cmd[0]==MFX_SET_STATION_ID)
+       {
+               if(length!=5)
+                       return LENGTH_ERROR;
+
+               uint32_t id = (cmd[1]<<8)|cmd[2];
+               id <<= 16;
+               id |= (uint16_t)(cmd[3]<<8)|cmd[4];
+               mfx_set_station_id(id);
+       }
+       else if(cmd[0]==MFX_ANNOUNCE)
+       {
+               if(length!=3)
+                       return LENGTH_ERROR;
+
+               mfx_announce_packet((cmd[1]<<8)|cmd[2]);
+       }
+       else if(cmd[0]==MFX_SEARCH)
+       {
+               if(length!=6)
+                       return LENGTH_ERROR;
+
+               uint8_t mask_size = cmd[5];
+               if(mask_size>0x20)
+                       return INVALID_VALUE;
+
+               uint32_t mask_bits = (cmd[1]<<8)|cmd[2];
+               mask_bits <<= 16;
+               mask_bits |= (uint16_t)(cmd[3]<<8)|cmd[4];
+               mfx_search_packet(mask_bits, mask_size);
+               mfx_receive_feedback();
+       }
+       else if(cmd[0]==MFX_ASSIGN_ADDRESS || cmd[0]==MFX_PING)
+       {
+               if(length!=7)
+                       return LENGTH_ERROR;
+
+               uint16_t addr = (cmd[1]<<8)|cmd[2];
+               if(addr==0 || addr>0x3FFF)
+                       return INVALID_VALUE;
+
+               uint32_t id = (cmd[3]<<8)|cmd[4];
+               id <<= 16;
+               id |= (uint16_t)(cmd[5]<<8)|cmd[6];
+               if(cmd[0]==MFX_ASSIGN_ADDRESS)
+                       mfx_assign_address_packet(addr, id);
+               else
+               {
+                       mfx_ping_packet(addr, id);
+                       mfx_receive_feedback();
+               }
+       }
+       else if(cmd[0]==MFX_SPEED || cmd[0]==MFX_SPEED_FUNCS8 || cmd[0]==MFX_SPEED_FUNCS16)
+       {
+               if(cmd[0]==MFX_SPEED && length!=4)
+                       return LENGTH_ERROR;
+               else if(cmd[0]==MFX_SPEED_FUNCS8 && length!=5)
+                       return LENGTH_ERROR;
+               else if(cmd[0]==MFX_SPEED_FUNCS16 && length!=6)
+                       return LENGTH_ERROR;
+
+               uint16_t addr = (cmd[1]<<8)|cmd[2];
+               if(addr==0 || addr>0x3FFF)
+                       return INVALID_VALUE;
+
+               uint8_t speed_dir = cmd[3];
+               if((speed_dir&0x7F)==0x7F)
+                       return INVALID_VALUE;
+               ++speed_dir;
+
+               if(cmd[0]==MFX_SPEED)
+                       mfx_speed_packet(addr, speed_dir);
+               else if(cmd[0]==MFX_SPEED_FUNCS8)
+                       mfx_speed_funcs8_packet(addr, speed_dir, cmd[4]);
+               else if(cmd[0]==MFX_SPEED_FUNCS16)
+                       mfx_speed_funcs8_packet(addr, speed_dir, (cmd[4]<<8)|cmd[5]);
+       }
+       else
+               return INVALID_COMMAND;
+
+       return COMMAND_OK;
+}
diff --git a/arducontrol/mfx.h b/arducontrol/mfx.h
new file mode 100644 (file)
index 0000000..d540184
--- /dev/null
@@ -0,0 +1,16 @@
+#ifndef MFX_H_
+#define MFX_H_
+
+#include <stdint.h>
+
+void mfx_set_station_id(uint32_t);
+void mfx_announce_packet(uint16_t);
+void mfx_search_packet(uint32_t, uint8_t);
+void mfx_assign_address_packet(uint16_t, uint32_t);
+void mfx_ping_packet(uint16_t, uint32_t);
+void mfx_speed_packet(uint16_t, uint8_t);
+void mfx_speed_funcs8_packet(uint16_t, uint8_t, uint8_t);
+void mfx_speed_funcs16_packet(uint16_t, uint8_t, uint16_t);
+uint8_t mfx_command(const uint8_t *, uint8_t);
+
+#endif