--- /dev/null
+#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;
+}
projects / model-railway-devices.git / commitdiff