viva_fake_gige/
gvcp_server.rs

1//! GVCP control channel server: discovery + GenCP register read/write.
2
3use std::net::SocketAddr;
4use std::sync::Arc;
5use std::sync::atomic::{AtomicBool, Ordering};
6
7use bytes::{BufMut, BytesMut};
8use tokio::net::UdpSocket;
9use tokio::sync::{Mutex, Notify};
10use tracing::{debug, trace, warn};
11
12use crate::registers::RegisterMap;
13
14/// GVCP command key byte (first byte of every GVCP command).
15const GVCP_CMD_KEY: u8 = 0x42;
16
17// GVCP command opcodes
18const DISCOVERY_CMD: u16 = 0x0002;
19const FORCEIP_CMD: u16 = 0x0004;
20const FORCEIP_ACK: u16 = 0x0005;
21const READREG_CMD: u16 = 0x0080;
22const WRITEREG_CMD: u16 = 0x0082;
23const READMEM_CMD: u16 = 0x0084;
24const WRITEMEM_CMD: u16 = 0x0086;
25/// Action command. Deliberately spelled out rather than reused from
26/// `viva-gige`: the fake must be able to disagree with the client, which is the
27/// whole point of asserting its bytes independently (ADR-0019).
28const ACTION_CMD: u16 = 0x0100;
29
30// GVCP ack opcodes
31const DISCOVERY_ACK: u16 = 0x0003;
32const READREG_ACK: u16 = 0x0081;
33const WRITEREG_ACK: u16 = 0x0083;
34const READMEM_ACK: u16 = 0x0085;
35const WRITEMEM_ACK: u16 = 0x0087;
36const ACTION_ACK: u16 = 0x0101;
37
38/// Action keys the fake accepts. A command whose keys do not match is ignored
39/// without an acknowledgement, which is how a real device behaves — the command
40/// is broadcast to every camera on the subnet and only the addressed group acts.
41pub const FAKE_DEVICE_KEY: u32 = 0x0000_0042;
42/// Group key the fake belongs to.
43pub const FAKE_GROUP_KEY: u32 = 0x0000_0001;
44/// Group mask bits the fake responds to.
45pub const FAKE_GROUP_MASK: u32 = 0x0000_0001;
46
47/// MAC address the fake camera reports. Public so tests can assert the exact
48/// bytes rather than merely that a MAC is present (ADR-0019).
49pub const FAKE_MAC: [u8; 6] = [0xDE, 0xAD, 0xBE, 0xEF, 0xCA, 0xFE];
50/// Manufacturer name reported in the Discovery ACK.
51pub const FAKE_MANUFACTURER: &str = "viva-genicam";
52/// Model name reported in the Discovery ACK.
53pub const FAKE_MODEL: &str = "FakeGigE";
54/// Device version reported in the Discovery ACK.
55pub const FAKE_VERSION: &str = "1.0.0";
56/// Serial number reported in the Discovery ACK.
57pub const FAKE_SERIAL: &str = "FAKE-001";
58/// User-defined name reported in the Discovery ACK.
59pub const FAKE_USER_NAME: &str = "FakeCamera";
60
61/// Status code for success.
62const STATUS_SUCCESS: u16 = 0x0000;
63/// GigE Vision `GEV_STATUS_INVALID_PARAMETER`.
64const STATUS_INVALID_PARAMETER: u16 = 0x8002;
65
66/// Run the GVCP control server loop.
67///
68/// Listens for GVCP commands and sends appropriate responses.
69/// Notifies `acq_notify` when AcquisitionStart is written.
70pub async fn run(
71    socket: Arc<UdpSocket>,
72    regs: Arc<Mutex<RegisterMap>>,
73    acq_start_notify: Arc<Notify>,
74    acq_stop_flag: Arc<AtomicBool>,
75    bind_ip: std::net::Ipv4Addr,
76) {
77    let mut buf = [0u8; 2048];
78    loop {
79        let (len, peer) = match socket.recv_from(&mut buf).await {
80            Ok(r) => r,
81            Err(e) => {
82                warn!(error = %e, "GVCP recv error");
83                continue;
84            }
85        };
86        let pkt = &buf[..len];
87        if len < 8 || pkt[0] != GVCP_CMD_KEY {
88            trace!(len, "ignoring non-GVCP packet");
89            continue;
90        }
91
92        let flags = pkt[1];
93        let command = u16::from_be_bytes([pkt[2], pkt[3]]);
94        let _length = u16::from_be_bytes([pkt[4], pkt[5]]);
95        let request_id = u16::from_be_bytes([pkt[6], pkt[7]]);
96        let payload = &pkt[8..];
97
98        // Register access refreshes the device's heartbeat timer. Discovery and
99        // FORCEIP do not: they are broadcast by any application on the subnet.
100        // The command is still served after an expiry — a real device answers
101        // register reads from a non-controller, it only refuses configuration.
102        if matches!(
103            command,
104            READREG_CMD | WRITEREG_CMD | READMEM_CMD | WRITEMEM_CMD
105        ) && regs.lock().await.note_register_command()
106        {
107            warn!(%peer, "heartbeat expired; control privilege released");
108        }
109
110        match command {
111            DISCOVERY_CMD => {
112                let resp = build_discovery_ack(request_id, bind_ip);
113                let _ = socket.send_to(&resp, peer).await;
114                debug!(%peer, "discovery response sent");
115            }
116            FORCEIP_CMD => {
117                handle_forceip(&socket, peer, request_id, payload, bind_ip).await;
118            }
119            READREG_CMD => {
120                handle_readreg(&socket, peer, request_id, payload, &regs).await;
121            }
122            WRITEREG_CMD => {
123                handle_writereg(
124                    &socket,
125                    peer,
126                    request_id,
127                    payload,
128                    &regs,
129                    &acq_start_notify,
130                    &acq_stop_flag,
131                )
132                .await;
133            }
134            READMEM_CMD => {
135                handle_readmem(&socket, peer, request_id, payload, &regs).await;
136            }
137            WRITEMEM_CMD => {
138                handle_writemem(
139                    &socket,
140                    peer,
141                    request_id,
142                    payload,
143                    &regs,
144                    &acq_start_notify,
145                    &acq_stop_flag,
146                )
147                .await;
148            }
149            ACTION_CMD => {
150                handle_action(&socket, peer, request_id, flags, payload).await;
151            }
152            _ => {
153                debug!(command, "unsupported GVCP command");
154            }
155        }
156    }
157}
158
159/// Build a 256-byte discovery ACK payload (GVCP header + device info).
160fn build_discovery_ack(request_id: u16, ip: std::net::Ipv4Addr) -> Vec<u8> {
161    // Discovery ack payload is 248 bytes (as defined by the GigE Vision spec).
162    let payload_len: u16 = 248;
163    let mut buf = BytesMut::with_capacity(8 + payload_len as usize);
164
165    // ACK header: status(2) + ack_cmd(2) + length(2) + request_id(2)
166    buf.put_u16(STATUS_SUCCESS);
167    buf.put_u16(DISCOVERY_ACK);
168    buf.put_u16(payload_len);
169    buf.put_u16(request_id);
170
171    // Discovery payload (248 bytes). Offsets below are payload-relative and
172    // come from the specification's field table, NOT from what our own parser
173    // happens to read — see ADR-0019. This layout previously placed the MAC at
174    // offset 12 because `parse_discovery_payload` read it there; both were
175    // wrong, and agreeing with each other is what hid it (#57).
176    buf.put_u16(2); // 0   Spec major version
177    buf.put_u16(0); // 2   Spec minor version
178    buf.put_u32(0); // 4   Device mode
179
180    // 8   Reserved: the padding half of the MAC-high register, 2 bytes only.
181    buf.put_slice(&[0u8; 2]);
182
183    // 10  MAC address (6 bytes): fake MAC DE:AD:BE:EF:CA:FE
184    buf.put_slice(&FAKE_MAC);
185
186    buf.put_u32(0x0000_0007); // 16  Supported IP config (DHCP + persistent + LLA)
187    buf.put_u32(0x0000_0005); // 20  Current IP config
188
189    // 24  Reserved (12 bytes)
190    buf.put_slice(&[0u8; 12]);
191
192    // 36  Current IP address
193    buf.put_slice(&ip.octets());
194
195    // 40  Reserved (12 bytes)
196    buf.put_slice(&[0u8; 12]);
197
198    // 52  Subnet mask (255.255.255.0)
199    buf.put_slice(&[255, 255, 255, 0]);
200
201    // 56  Reserved (12 bytes)
202    buf.put_slice(&[0u8; 12]);
203
204    // 68  Gateway
205    buf.put_slice(&[0, 0, 0, 0]);
206
207    // Manufacturer name (32 bytes)
208    put_fixed_string(&mut buf, FAKE_MANUFACTURER, 32); // 72
209    // Model name (32 bytes)
210    put_fixed_string(&mut buf, FAKE_MODEL, 32); // 104
211    // Device version (32 bytes)
212    put_fixed_string(&mut buf, FAKE_VERSION, 32); // 136
213    // Manufacturer specific info (48 bytes)
214    put_fixed_string(&mut buf, "Fake camera for testing", 48); // 168
215    // Serial number (16 bytes)
216    put_fixed_string(&mut buf, FAKE_SERIAL, 16); // 216
217    // User defined name (16 bytes)
218    put_fixed_string(&mut buf, FAKE_USER_NAME, 16); // 232
219
220    buf.to_vec()
221}
222
223fn put_fixed_string(buf: &mut BytesMut, s: &str, len: usize) {
224    let bytes = s.as_bytes();
225    let copy_len = bytes.len().min(len);
226    buf.put_slice(&bytes[..copy_len]);
227    for _ in copy_len..len {
228        buf.put_u8(0);
229    }
230}
231
232/// Build a generic GVCP ACK header + payload.
233fn build_ack(ack_cmd: u16, request_id: u16, payload: &[u8]) -> Vec<u8> {
234    let mut buf = BytesMut::with_capacity(8 + payload.len());
235    buf.put_u16(STATUS_SUCCESS);
236    buf.put_u16(ack_cmd);
237    buf.put_u16(payload.len() as u16);
238    buf.put_u16(request_id);
239    buf.put_slice(payload);
240    buf.to_vec()
241}
242
243/// Build a payload-less error ACK (8-byte header only), as real cameras send.
244fn build_error_ack(ack_cmd: u16, request_id: u16, status: u16) -> Vec<u8> {
245    let mut buf = BytesMut::with_capacity(8);
246    buf.put_u16(status);
247    buf.put_u16(ack_cmd);
248    buf.put_u16(0);
249    buf.put_u16(request_id);
250    buf.to_vec()
251}
252
253async fn handle_readreg(
254    socket: &UdpSocket,
255    peer: SocketAddr,
256    request_id: u16,
257    payload: &[u8],
258    regs: &Mutex<RegisterMap>,
259) {
260    // READREG payload: one or more 4-byte addresses
261    if payload.len() < 4 || !payload.len().is_multiple_of(4) {
262        return;
263    }
264    let store = regs.lock().await;
265    let mut resp_payload = BytesMut::new();
266    for chunk in payload.chunks(4) {
267        let addr = u32::from_be_bytes([chunk[0], chunk[1], chunk[2], chunk[3]]) as u64;
268        let data = store.read(addr, 4);
269        resp_payload.put_slice(&data);
270    }
271    let resp = build_ack(READREG_ACK, request_id, &resp_payload);
272    let _ = socket.send_to(&resp, peer).await;
273    trace!(%peer, regs = payload.len() / 4, "READREG response");
274}
275
276async fn handle_writereg(
277    socket: &UdpSocket,
278    peer: SocketAddr,
279    request_id: u16,
280    payload: &[u8],
281    regs: &Mutex<RegisterMap>,
282    acq_start: &Notify,
283    acq_stop_flag: &AtomicBool,
284) {
285    // WRITEREG payload: pairs of (address: u32, value: u32)
286    if payload.len() < 8 || !payload.len().is_multiple_of(8) {
287        return;
288    }
289    let mut store = regs.lock().await;
290    for chunk in payload.chunks(8) {
291        let addr = u32::from_be_bytes([chunk[0], chunk[1], chunk[2], chunk[3]]) as u64;
292        let value = &chunk[4..8];
293        store.write(addr, value);
294        store.handle_special_write(addr);
295        check_acquisition(addr, value, acq_start, acq_stop_flag);
296    }
297    // WRITEREG ACK includes a 4-byte data index placeholder.
298    let resp = build_ack(WRITEREG_ACK, request_id, &[0, 0, 0, 0]);
299    let _ = socket.send_to(&resp, peer).await;
300    trace!(%peer, "WRITEREG response");
301}
302
303async fn handle_readmem(
304    socket: &UdpSocket,
305    peer: SocketAddr,
306    request_id: u16,
307    payload: &[u8],
308    regs: &Mutex<RegisterMap>,
309) {
310    // READMEM payload: address(4) + reserved(2) + count(2)
311    if payload.len() < 8 {
312        return;
313    }
314    let addr = u32::from_be_bytes([payload[0], payload[1], payload[2], payload[3]]) as u64;
315    let count = u16::from_be_bytes([payload[6], payload[7]]) as usize;
316
317    // GVCP requires the address and byte count to be multiples of 4. Real
318    // cameras (e.g. Hikrobot) reject violations with INVALID_PARAMETER and a
319    // bare 8-byte ack header; be equally strict so that client bugs are
320    // caught by the in-tree fake (regression guard for issue #35).
321    if !addr.is_multiple_of(4) || count == 0 || !count.is_multiple_of(4) {
322        let resp = build_error_ack(READMEM_ACK, request_id, STATUS_INVALID_PARAMETER);
323        let _ = socket.send_to(&resp, peer).await;
324        debug!(%peer, addr = format!("0x{addr:x}"), count, "READMEM rejected: unaligned");
325        return;
326    }
327
328    let store = regs.lock().await;
329    let data = store.read(addr, count);
330
331    // READMEM ACK payload: address(4) + data(N)
332    let mut resp_payload = BytesMut::with_capacity(4 + data.len());
333    resp_payload.put_u32(addr as u32);
334    resp_payload.put_slice(&data);
335    let resp = build_ack(READMEM_ACK, request_id, &resp_payload);
336    let _ = socket.send_to(&resp, peer).await;
337    trace!(%peer, addr = format!("0x{addr:x}"), count, "READMEM response");
338}
339
340async fn handle_writemem(
341    socket: &UdpSocket,
342    peer: SocketAddr,
343    request_id: u16,
344    payload: &[u8],
345    regs: &Mutex<RegisterMap>,
346    acq_start: &Notify,
347    acq_stop_flag: &AtomicBool,
348) {
349    // WRITEMEM payload: address(4) + data(N)
350    if payload.len() < 4 {
351        return;
352    }
353    let addr = u32::from_be_bytes([payload[0], payload[1], payload[2], payload[3]]) as u64;
354    let data = &payload[4..];
355    let (test_packet, dest_ip, dest_port, max_on_wire) = {
356        let mut store = regs.lock().await;
357        store.write(addr, data);
358        store.handle_special_write(addr);
359        check_acquisition(addr, data, acq_start, acq_stop_flag);
360        (
361            store.take_pending_test_packet(),
362            store.stream_dest_ip(),
363            store.stream_dest_port(),
364            store.max_on_wire(),
365        )
366    };
367
368    // WRITEMEM ACK payload: address(4)
369    let mut resp_payload = BytesMut::with_capacity(4);
370    resp_payload.put_u32(addr as u32);
371    let resp = build_ack(WRITEMEM_ACK, request_id, &resp_payload);
372    let _ = socket.send_to(&resp, peer).await;
373
374    // The acknowledgement goes first: a real device answers the write and then
375    // emits the test packet, and a controller that waits for the ack before
376    // listening must not miss it.
377    if let Some(size) = test_packet {
378        fire_test_packet(size, dest_ip, dest_port, max_on_wire).await;
379    }
380    trace!(%peer, addr = format!("0x{addr:x}"), len = data.len(), "WRITEMEM response");
381}
382
383/// Emit one GVSP test packet of `size` bytes to the configured stream
384/// destination, unless the configured path ceiling would have swallowed it.
385///
386/// `size` is the IP datagram size the controller asked for, so the UDP payload
387/// is `size` minus the 20-byte IPv4 and 8-byte UDP headers. What matters to the
388/// probe is only whether *something* arrives, but sending the right length lets
389/// a caller check it.
390///
391/// The `max_on_wire` arm is the point of the whole mechanism: a path that
392/// cannot carry the frame drops it with nobody to report the loss, which is
393/// exactly what a register read cannot discover
394/// ([#112](https://github.com/VitalyVorobyev/viva-genicam/issues/112)).
395async fn fire_test_packet(
396    size: u32,
397    dest_ip: std::net::Ipv4Addr,
398    dest_port: u16,
399    max_on_wire: Option<u32>,
400) {
401    if dest_port == 0 {
402        debug!("test packet requested before a stream destination was set");
403        return;
404    }
405    if max_on_wire.is_some_and(|max| size > max) {
406        debug!(
407            size,
408            "test packet exceeds the path ceiling; dropped in flight"
409        );
410        return;
411    }
412
413    const IP_AND_UDP_HEADERS: u32 = 28;
414    let payload_len = size.saturating_sub(IP_AND_UDP_HEADERS) as usize;
415    let Ok(sock) = UdpSocket::bind("0.0.0.0:0").await else {
416        return;
417    };
418    // A test packet is a GVSP payload-format datagram; the controller only
419    // checks that it arrived, so the header just has to be well formed.
420    let mut pkt = BytesMut::with_capacity(payload_len.max(8));
421    pkt.put_u16(0); // status
422    pkt.put_u16(0); // block id — a test packet belongs to no block
423    pkt.put_u8(0x03); // packet format: payload
424    pkt.put_u8(0);
425    pkt.put_u16(0); // packet id
426    pkt.resize(payload_len.max(8), 0);
427    let _ = sock.send_to(&pkt, (dest_ip, dest_port)).await;
428    debug!(size, %dest_ip, dest_port, "test packet sent");
429}
430
431/// Handle a GVCP `ACTION_CMD` (0x0100).
432///
433/// Payload is `device_key`, `group_key`, `group_mask` — 12 bytes — plus a
434/// 64-bit action time when flags bit 7 is set. A 24-byte payload, or one
435/// carrying a scheduled time without the flag, is malformed and gets no reply:
436/// the client used to send exactly that, under opcode 0x0080 (`READREG`).
437async fn handle_action(
438    socket: &UdpSocket,
439    peer: SocketAddr,
440    request_id: u16,
441    flags: u8,
442    payload: &[u8],
443) {
444    const SCHEDULED: u8 = 0x80;
445    const ACK_REQUIRED: u8 = 0x01;
446
447    let scheduled = flags & SCHEDULED != 0;
448    let expected = if scheduled { 20 } else { 12 };
449    if payload.len() < expected {
450        warn!(
451            len = payload.len(),
452            expected, scheduled, "malformed action command payload"
453        );
454        return;
455    }
456
457    let device_key = u32::from_be_bytes([payload[0], payload[1], payload[2], payload[3]]);
458    let group_key = u32::from_be_bytes([payload[4], payload[5], payload[6], payload[7]]);
459    let group_mask = u32::from_be_bytes([payload[8], payload[9], payload[10], payload[11]]);
460
461    if device_key != FAKE_DEVICE_KEY
462        || group_key != FAKE_GROUP_KEY
463        || group_mask & FAKE_GROUP_MASK == 0
464    {
465        debug!(
466            device_key,
467            group_key, group_mask, "action command not addressed to this device"
468        );
469        return;
470    }
471
472    debug!(%peer, scheduled, "action command accepted");
473    if flags & ACK_REQUIRED != 0 {
474        let mut buf = BytesMut::with_capacity(8);
475        buf.put_u16(STATUS_SUCCESS);
476        buf.put_u16(ACTION_ACK);
477        buf.put_u16(0);
478        buf.put_u16(request_id);
479        let _ = socket.send_to(&buf, peer).await;
480    }
481}
482
483async fn handle_forceip(
484    socket: &UdpSocket,
485    peer: SocketAddr,
486    request_id: u16,
487    payload: &[u8],
488    bind_ip: std::net::Ipv4Addr,
489) {
490    // FORCEIP payload: 56 bytes
491    // [0..2]   reserved
492    // [2..8]   target MAC address
493    // [8..20]  reserved
494    // [20..24] static IP
495    // [24..36] reserved
496    // [36..40] subnet mask
497    // [40..52] reserved
498    // [52..56] gateway
499    if payload.len() < 56 {
500        warn!(len = payload.len(), "FORCEIP payload too short");
501        return;
502    }
503
504    let target_mac = &payload[2..8];
505    let fake_mac: [u8; 6] = FAKE_MAC;
506    if target_mac != fake_mac {
507        debug!(
508            target = ?target_mac,
509            "FORCEIP: MAC mismatch, ignoring"
510        );
511        return;
512    }
513
514    let ip = std::net::Ipv4Addr::new(payload[20], payload[21], payload[22], payload[23]);
515    let subnet = std::net::Ipv4Addr::new(payload[36], payload[37], payload[38], payload[39]);
516    let gateway = std::net::Ipv4Addr::new(payload[52], payload[53], payload[54], payload[55]);
517
518    debug!(
519        %bind_ip,
520        %ip,
521        %subnet,
522        %gateway,
523        "FORCEIP accepted (fake camera ignores IP change)"
524    );
525
526    // Send FORCEIP_ACK (empty payload).
527    let resp = build_ack(FORCEIP_ACK, request_id, &[]);
528    let _ = socket.send_to(&resp, peer).await;
529}
530
531/// Check if a write targets an acquisition register and notify accordingly.
532fn check_acquisition(addr: u64, data: &[u8], acq_start: &Notify, acq_stop_flag: &AtomicBool) {
533    use crate::registers::{REG_ACQ_START, REG_ACQ_STOP};
534
535    if addr == REG_ACQ_START && data.len() >= 4 {
536        let val = u32::from_be_bytes([data[0], data[1], data[2], data[3]]);
537        if val != 0 {
538            debug!("AcquisitionStart triggered");
539            acq_stop_flag.store(false, Ordering::SeqCst);
540            acq_start.notify_one();
541        }
542    } else if addr == REG_ACQ_STOP && data.len() >= 4 {
543        let val = u32::from_be_bytes([data[0], data[1], data[2], data[3]]);
544        if val != 0 {
545            debug!("AcquisitionStop triggered");
546            acq_stop_flag.store(true, Ordering::SeqCst);
547        }
548    }
549}
viva_fake_gige/gvcp_server.rs

viva_fake_gige/
gvcp_server.rs
