-
Notifications
You must be signed in to change notification settings - Fork 55
/
protocol.ml
84 lines (72 loc) · 2.73 KB
/
protocol.ml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
(*
* Generic communication protocol handling
*
* Copyright (C) 2004 CENA/ENAC, Pascal Brisset
* Copyright (C) 2015 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*)
open Printf
type payload = string
let string_of_payload = fun x -> x
let payload_of_string = fun x -> x
exception Not_enough
module type PROTOCOL = sig
val index_start : string -> int (* raise Not_found *)
val length : string -> int -> int (* raise Not_enough *)
val checksum : string -> bool
val payload : string -> payload
val packet : payload -> string
end
module Transport(Protocol:PROTOCOL) = struct
let nb_err = ref 0
let discarded_bytes = ref 0
let rec parse = fun use buf ->
DebugPL.call 'T' (fun f -> fprintf f "Transport.parse: %s\n" (DebugPL.xprint buf));
(** ref required due to Not_enough exception raised by Protocol.length *)
let start = ref 0
and n = CompatPL.bytes_length buf in
try
(* Looks for the beginning of the frame. May raise Not_found exception *)
start := Protocol.index_start buf;
(* Discards skipped characters *)
discarded_bytes := !discarded_bytes + !start;
(* Get length of the frame (may raise Not_enough exception) *)
let length = Protocol.length buf !start in
let end_ = !start + length in
(* Checks if the complete frame is available in the buffer. *)
if n < end_ then
raise Not_enough;
(* Extracts the complete frame *)
let msg = CompatPL.bytes_sub buf !start length in
(* Checks sum *)
if Protocol.checksum msg then begin
(* Calls the handler with the message *)
use (Protocol.payload msg)
end else begin
(* Reports the error *)
incr nb_err;
discarded_bytes := !discarded_bytes + length;
DebugPL.call 'T' (fun f -> fprintf f "Transport.chk: %s\n" (DebugPL.xprint msg))
end;
(* Continues with the rest of the message *)
end_ + parse use (CompatPL.bytes_sub buf end_ (CompatPL.bytes_length buf - end_))
with
Not_found -> CompatPL.bytes_length buf
| Not_enough -> !start
end