packet/utils.lisp at master · fjames86/packet · GitHub

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
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
;;
;; Package for packing/unpacking strutures into/out of Lisp buffers.
;; Can be used for e.g. UDP packets or mmap'ed data structures.
;;
;; Copyright Frank James
;; March 2014
;;


(in-package :packet)


(defun subseq* (sequence start &optional len)
  "Subsequence with length"
  (subseq sequence start (when len (+ start len))))

(defun pad (array len)
  "Pad array with zeros if its too short"
  (let* ((l (length array))
         (arr (make-array (max len l) :initial-element 0 :element-type '(unsigned-byte 8))))
    (dotimes (i (length arr))
      (when (< i l)
        (setf (elt arr i) (elt array i))))
    arr))

(defun pad* (array &optional (width 4))
  "Pad to a length multiple of WIDTH"
  (let* ((l (length array))
	 (m (mod l width)))
    (if (zerop m)
	array
	(pad array (+ l (- width m))))))

(defun hd (data)
  "Hexdump output"
  (let ((lbuff (make-array 16))
        (len (length data)))
    (labels ((pline (lbuff count)
               (dotimes (i count)
                 (format t " ~2,'0X" (svref lbuff i)))
               (dotimes (i (- 16 count))
                 (format t "   "))

               (format t " | ")
               (dotimes (i count)
                 (let ((char (code-char (svref lbuff i))))
                   (format t "~C"
                           (if (graphic-char-p char) char #\.))))
               (terpri)))
      (do ((pos 0 (+ pos 16)))
          ((>= pos len))
        (let ((count (min 16 (- len pos))))
          (dotimes (i count)
            (setf (svref lbuff i) (elt data (+ pos i))))
          (format t "; ~8,'0X:  " pos)
          (pline lbuff count))))))


(defun usb8 (&rest sequences)
  "Make an (unsigned byte 8) vector from the sequences"
  (apply #'concatenate '(vector (unsigned-byte 8)) sequences))

(defun usb8* (&rest numbers)
  "Make an (unsigned-byte 8) vector from the numbers"
  (make-array (length numbers)
              :element-type '(unsigned-byte 8)
              :initial-contents numbers))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Flags
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defmacro defflags (name flags &optional documentation)
  "Macro to define a set of flags"
  `(eval-when (:compile-toplevel :load-toplevel :execute)
     (defparameter ,name
       (list ,@(mapcar (lambda (flag)
                         (destructuring-bind (n v &optional doc) flag
                           (let ((gv (gensym)))
                             `(let ((,gv ,v))
                                (list ',n (ash 1 ,gv) ,gv ,doc)))))
                       flags))
     ,documentation)))

(defun pack-flags (flag-names flags)
  "Combine flags"
  (let ((f 0))
    (dolist (flag-name flag-names)
      (let ((n (cadr (assoc flag-name flags))))
        (unless n (error "Flag ~S not found" flag-name))
        (setf f (logior f n))))
    f))

(defun unpack-flags (number flags)
  "Split the number into its flags."
  (let ((f nil)
        (num number))
    (dolist (flag flags)
      (let ((n (cadr flag)))
        (unless (zerop (logand number n))
          (push (car flag) f)
          (setf num (logand num (lognot n))))))
    (unless (zerop num)
      (warn "Input flags ~S remainder ~S" number num))
;;    (assert (zerop number))
    f))

(defun flag-p (number flag-name flags)
  (let ((flag (assoc flag-name flags)))
    (unless flag (error "No flag ~S" flag-name))
    (not (zerop (logand number (cadr flag))))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Enums
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defmacro defenum (name enums)
  "Define a list of enums"
  `(eval-when (:compile-toplevel :load-toplevel :execute)
     (defparameter ,name
       (list ,@(let ((i 0))
                    (mapcar (lambda (enum)
                              (cond
                                ((symbolp enum)
                                 (prog1 `(list ',enum ,i nil)
                                   (incf i)))
                                (t
                                 (destructuring-bind (n v &optional doc) enum
                                   (prog1 `(list ',n ,v ,doc)
                                     (setf i (1+ v)))))))
                            enums))))))

(defun enum-p (number enum enums)
  (let ((e (assoc enum enums)))
    (unless e (error "No such enum ~S" enum))
    (= number (cadr e))))

(defun enum (enum enums)
  (let ((e (assoc enum enums)))
    (unless e (error "No such enum ~S" enum))
    (cadr e)))

(defun enum-id (number enums)
  (car (find number enums :key #'cadr)))