add everything im working on
This commit is contained in:
parent
7bf784e6bc
commit
0298299402
|
|
@ -0,0 +1,26 @@
|
||||||
|
Licenses for other components:
|
||||||
|
|
||||||
|
software/src/jsmn.h
|
||||||
|
===================
|
||||||
|
|
||||||
|
MIT License
|
||||||
|
|
||||||
|
Copyright (c) 2010 Serge Zaitsev
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||||
|
copies of the Software, and to permit persons to whom the Software is
|
||||||
|
furnished to do so, subject to the following conditions:
|
||||||
|
|
||||||
|
The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||||
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||||
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||||
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||||
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||||
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||||
|
SOFTWARE.
|
||||||
|
|
@ -0,0 +1,107 @@
|
||||||
|
/* Booth Multiplication v0.1
|
||||||
|
* Written by Peter McGoron, 2022.
|
||||||
|
*/
|
||||||
|
|
||||||
|
module boothmul
|
||||||
|
#(
|
||||||
|
parameter A1_LEN = 32,
|
||||||
|
parameter A2_LEN = 32,
|
||||||
|
// AZLEN_SIZ = floor(log2(A2_LEN + 2) + 1).
|
||||||
|
// It must be able to store A2_LEN + 2.
|
||||||
|
parameter A2LEN_SIZ = 6
|
||||||
|
)
|
||||||
|
(
|
||||||
|
input clk,
|
||||||
|
input arm,
|
||||||
|
input [A1_LEN-1:0] a1,
|
||||||
|
input [A2_LEN-1:0] a2,
|
||||||
|
output [A1_LEN+A2_LEN-1:0] outn,
|
||||||
|
output reg fin
|
||||||
|
);
|
||||||
|
|
||||||
|
/***********************
|
||||||
|
* Booth Parameters
|
||||||
|
**********************/
|
||||||
|
|
||||||
|
localparam OUT_LEN = A1_LEN + A2_LEN;
|
||||||
|
localparam REG_LEN = OUT_LEN + 2;
|
||||||
|
|
||||||
|
/* The Booth multiplication algorithm is a sequential algorithm for
|
||||||
|
* twos-compliment integers.
|
||||||
|
*
|
||||||
|
* Let REG_LEN be equal to 1 + len(a1) + len(a2) + 1.
|
||||||
|
* Let P, S, and A be of length REG_LEN.
|
||||||
|
* Let A = a1 << len(a2) + 1, where a1 sign extends to the upper bit.
|
||||||
|
* Let S = -a1 << len(a2) + 1, where a1 sign extens to the upper bit.
|
||||||
|
* Let P = a2 << 1.
|
||||||
|
*
|
||||||
|
* Repeat the following len(a2) times:
|
||||||
|
* case(P[1:0])
|
||||||
|
* 2'b00, 2'b11: P <= P >>> 1;
|
||||||
|
* 2'b01: P <= (P + A) >>> 1;
|
||||||
|
* 2'b10: P <= (P + S) >>> 1;
|
||||||
|
* endcase
|
||||||
|
* The final value is P[REG_LEN-2:1].
|
||||||
|
*
|
||||||
|
* Wires and registers of REG_LEN length are organized like:
|
||||||
|
*
|
||||||
|
* /Overflow bit
|
||||||
|
* [M][ REG_LEN ][0]
|
||||||
|
* [M][ A1_LEN ][ A2_LEN ][0]
|
||||||
|
*/
|
||||||
|
|
||||||
|
reg signed [REG_LEN-1:0] a;
|
||||||
|
reg signed [REG_LEN-1:0] s;
|
||||||
|
reg signed [REG_LEN-1:0] p = 0;
|
||||||
|
|
||||||
|
assign outn[OUT_LEN-1:0] = p[REG_LEN-2:1];
|
||||||
|
|
||||||
|
/**********************
|
||||||
|
* Loop Implementation
|
||||||
|
*********************/
|
||||||
|
|
||||||
|
reg[A2LEN_SIZ-1:0] loop_accul = 0;
|
||||||
|
|
||||||
|
always @ (posedge clk) begin
|
||||||
|
if (!arm) begin
|
||||||
|
loop_accul <= 0;
|
||||||
|
fin <= 0;
|
||||||
|
end else if (loop_accul == 0) begin
|
||||||
|
p[0] <= 0;
|
||||||
|
p[A2_LEN:1] <= a2;
|
||||||
|
p[REG_LEN-1:A2_LEN+1] <= 0;
|
||||||
|
|
||||||
|
a[A2_LEN:0] <= 0;
|
||||||
|
a[REG_LEN-2:A2_LEN + 1] <= a1;
|
||||||
|
a[REG_LEN-1] <= a1[A1_LEN-1]; // Sign extension
|
||||||
|
|
||||||
|
s[A2_LEN:0] <= 0;
|
||||||
|
// Extend before negation to ensure size
|
||||||
|
s[REG_LEN-1:A2_LEN+1] <= ~{a1[A1_LEN-1],a1} + 1;
|
||||||
|
|
||||||
|
loop_accul <= loop_accul + 1;
|
||||||
|
end else if (loop_accul < A2_LEN + 1) begin
|
||||||
|
/* The loop counter starts from 1, so it must go to
|
||||||
|
* A2_LEN + 1 exclusive.
|
||||||
|
* (i = 0; i < len; i++)
|
||||||
|
* becomes (i = 1; i < len + 1; i++)
|
||||||
|
*/
|
||||||
|
loop_accul <= loop_accul + 1;
|
||||||
|
case (p[1:0])
|
||||||
|
2'b00, 2'b11: p <= p >>> 1;
|
||||||
|
2'b10: p <= (p + s) >>> 1;
|
||||||
|
2'b01: p <= (p + a) >>> 1;
|
||||||
|
endcase
|
||||||
|
end else begin
|
||||||
|
fin <= 1;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
`ifdef BOOTH_SIM
|
||||||
|
initial begin
|
||||||
|
$dumpfile("booth.vcd");
|
||||||
|
$dumpvars;
|
||||||
|
end
|
||||||
|
`endif
|
||||||
|
|
||||||
|
endmodule
|
||||||
|
|
@ -0,0 +1,396 @@
|
||||||
|
/************ Introduction to PI Controllers
|
||||||
|
* The continuous form of a PI loop is
|
||||||
|
*
|
||||||
|
* A(t) = P e(t) + I∫ e(t')dt'
|
||||||
|
* where e(t) is the error (setpoint - measured), and
|
||||||
|
* the integral goes from 0 to the current time 't'.
|
||||||
|
*
|
||||||
|
* In digital systems the integral must be approximated.
|
||||||
|
* The normal way of doing this is a first-order approximation
|
||||||
|
* of the derivative of A(t).
|
||||||
|
*
|
||||||
|
* dA(t)/dt = P de(t)/dt + Ie(t)
|
||||||
|
* A(t_n) - A(t_{n-1}) ≅ P (e(t_n) - e(t_{n-1})) + Ie(t_n)Δt
|
||||||
|
* A(t_n) ≅ A(t_{n-1}) + e(t_n)(P + IΔt) - Pe(t_{n-1})
|
||||||
|
*
|
||||||
|
* Using α = P + IΔt, and denoting A(t_{n-1}) as A_p,
|
||||||
|
*
|
||||||
|
* A ≅ A_p + αe - Pe_p.
|
||||||
|
*
|
||||||
|
* The formula above is what this module implements. This way,
|
||||||
|
* the controller only has to store two values between each
|
||||||
|
* run of the loop: the previous error and the previous output.
|
||||||
|
* This also reduces the amount of (redundant) computations
|
||||||
|
* the loop must execute each iteration.
|
||||||
|
*
|
||||||
|
* Calculating α requires knowing the precise timing of each
|
||||||
|
* control loop cycle, which in turn requires knowing the
|
||||||
|
* ADC and DAC timings. This is done outside the Verilog code.
|
||||||
|
* and can be calculated from simulating one iteration of the
|
||||||
|
* control loop.
|
||||||
|
*
|
||||||
|
*************** Fixed Point Integers *************
|
||||||
|
* A regular number is stored in decimal: 123056.
|
||||||
|
* This is equal to
|
||||||
|
* 6*10^0 + 5*10^1 + 0*10^2 + 3*10^3 + 2*10^4 + 1*10^5.
|
||||||
|
* A whole binary number is only ones and zeros: 1101, and is
|
||||||
|
* equal to
|
||||||
|
* 1*2^0 + 0*2^1 + 1*2^2 + 1*2^3.
|
||||||
|
*
|
||||||
|
* Fixed-point integers shift the exponent of each number by a
|
||||||
|
* fixed amount. For instance, 123.056 is
|
||||||
|
* 6*10^-3 + 5*10^-2 + 0*10^-1 + 3*10^0 + 2*10^1 + 1*10^2.
|
||||||
|
* Similarly, the fixed point binary integer 11.01 is
|
||||||
|
* 1*2^-2 + 0*2^-1 + 1*2^0 + 1*2^1.
|
||||||
|
*
|
||||||
|
* To a computer, a whole binary number and a fixed point binary
|
||||||
|
* number are stored in exactly the same way: no decimal point
|
||||||
|
* is stored. It is only the *interpretation* of the data that
|
||||||
|
* changes.
|
||||||
|
*
|
||||||
|
* Fixed point numbers are denoted WHOLE.FRAC or [WHOLE].[FRAC],
|
||||||
|
* where WHOLE is the amount of whole number bits (including sign)
|
||||||
|
* and FRAC is the amount of fractional bits (2^-1, 2^-2, etc.).
|
||||||
|
*
|
||||||
|
* The rules for how many digits the output has given an input
|
||||||
|
* is the same for fixed point binary and regular decimals.
|
||||||
|
*
|
||||||
|
* Addition: W1.F1 + W2.F2 = [max(W1,W2)+1].[max(F1,F2)]
|
||||||
|
* Multiplication: W1.F1 * W2.F2 = [W1+W2].[F1+F2]
|
||||||
|
*
|
||||||
|
*************** Precision **************
|
||||||
|
* The control loop is designed around these values, but generally
|
||||||
|
* does not hardcode them.
|
||||||
|
*
|
||||||
|
* Since α and P are precalculated outside of the loop, their
|
||||||
|
* conversion to numbers the loop understands is done outside of
|
||||||
|
* the loop and in the kernel.
|
||||||
|
*
|
||||||
|
* The 18-bit ADC is twos-compliment, -10.24V to 10.24V,
|
||||||
|
* with 78μV per increment.
|
||||||
|
* The 20-bit DAC is twos-compliment, -10V to 10V.
|
||||||
|
*
|
||||||
|
* The `P` constant has a minimum value of 1e-7 with a precision
|
||||||
|
* of 1e-9, and a maxmimum value of 1.
|
||||||
|
*
|
||||||
|
* The `I` constant has a minimum value of 1e-4 with a precision
|
||||||
|
* of 1e-6 and a maximum value of 100.
|
||||||
|
*
|
||||||
|
* Δt is cycles/100MHz. This makes Δt at least 10 ns, with a
|
||||||
|
* maximum of 1 ms.
|
||||||
|
*
|
||||||
|
* Intermediate values are 48-bit fixed-point integers multiplied
|
||||||
|
* by the step size of the ADC. The first 18 bits are the whole
|
||||||
|
* number and sign bits. This means intermediate values correspond
|
||||||
|
* exactly to values as understood by the ADC, with extra precision.
|
||||||
|
*
|
||||||
|
* To get the normal fixed-point value of an intermediate value,
|
||||||
|
* multiply it by 78e-6. To convert a normal fixed-point integer
|
||||||
|
* to an intermediate value, multiply it by 1/78e-6. In both
|
||||||
|
* cases, the conversion constant is a normal fixed-point integer.
|
||||||
|
*
|
||||||
|
* For instance, to encode the value 78e-6 as an intermediate
|
||||||
|
* value, multiply it by 1/78e-6 to obtain 1. Thus the value
|
||||||
|
* should be stored as 1 (whole bit) followed by zeros (fractional
|
||||||
|
* bits).
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 0x3214*78e-6 = 0.9996 + lower order (storable as 15 whole bits)
|
||||||
|
*/
|
||||||
|
|
||||||
|
`define ERR_WID (ADC_WID + 1)
|
||||||
|
|
||||||
|
module control_loop
|
||||||
|
#(
|
||||||
|
parameter ADC_WID = 18,
|
||||||
|
/* Code assumes DAC_WID > ADC_WID. If/when this is not the
|
||||||
|
* case, truncation code must be changed.
|
||||||
|
*/
|
||||||
|
parameter DAC_WID = 24,
|
||||||
|
/* Analog Devices DACs have a register code in the upper 4 bits.
|
||||||
|
* The data follows it.
|
||||||
|
*/
|
||||||
|
parameter DAC_DATA_WID = 20,
|
||||||
|
parameter CONSTS_WID = 48, // larger than ADC_WID
|
||||||
|
parameter CONSTS_FRAC_WID = CONSTS_WID-15,
|
||||||
|
parameter DELAY_WID = 16,
|
||||||
|
/* [ERR_WID_SIZ-1:0] must be able to store
|
||||||
|
* ERR_WID (ADC_WID + 1).
|
||||||
|
*/
|
||||||
|
parameter ERR_WID_SIZ = 6,
|
||||||
|
parameter ADC_POLARITY = 1,
|
||||||
|
parameter ADC_PHASE = 0,
|
||||||
|
parameter DAC_POLARITY = 0,
|
||||||
|
parameter DAC_PHASE = 1
|
||||||
|
) (
|
||||||
|
input clk,
|
||||||
|
input arm,
|
||||||
|
|
||||||
|
output adc_sck,
|
||||||
|
input adc_in,
|
||||||
|
output adc_conv,
|
||||||
|
|
||||||
|
output dac_sck,
|
||||||
|
output dac_ss,
|
||||||
|
output dac_out,
|
||||||
|
|
||||||
|
/* Informational output.
|
||||||
|
* These registers are also used for storing information while
|
||||||
|
* the loop is running.
|
||||||
|
*/
|
||||||
|
output signed [ERR_WID-1:0] err_cur,
|
||||||
|
output signed [CONSTS_WID-1:0] adj,
|
||||||
|
|
||||||
|
input signed [ADC_WID-1:0] setpt_in,
|
||||||
|
input signed [CONSTS_WID-1:0] cl_alpha_in,
|
||||||
|
input signed [CONSTS_WID-1:0] cl_p_in,
|
||||||
|
input [DELAY_WID-1:0] delay_in
|
||||||
|
);
|
||||||
|
|
||||||
|
/* Registers used to lock in values at the start of each iteration */
|
||||||
|
reg signed [ADC_WID-1:0] setpt = 0;
|
||||||
|
reg signed [CONSTS_WID-1:0] cl_alpha_reg = 0;
|
||||||
|
reg signed [CONSTS_WID-1:0] cl_p_reg = 0;
|
||||||
|
reg [DELAY_WID-1:0] saved_delay = 0;
|
||||||
|
|
||||||
|
/* Registers for PI calculations */
|
||||||
|
reg signed [ERR_WID-1:0] err_prev = 0;
|
||||||
|
|
||||||
|
/****** State machine
|
||||||
|
*
|
||||||
|
* -> WAIT_ON_ARM -> WAIT_ON_ADC -> WAIT_ON_MUL -\
|
||||||
|
* \------\------------ WAIT_ON_DAC </
|
||||||
|
*
|
||||||
|
* The loop will stop and reset all stored data if `arm` is not 1 at
|
||||||
|
* the end of the loop.
|
||||||
|
* The loop stores all data from the input into registers at
|
||||||
|
* `WAIT_ON_ADC`, so the program can change constants and setpoints
|
||||||
|
* on the fly.
|
||||||
|
*/
|
||||||
|
|
||||||
|
localparam WAIT_ON_ARM = 0;
|
||||||
|
localparam WAIT_ON_ADC = 1;
|
||||||
|
localparam WAIT_ON_MUL = 2;
|
||||||
|
localparam WAIT_ON_DAC = 3;
|
||||||
|
localparam STATESIZ = 2;
|
||||||
|
|
||||||
|
reg [STATESIZ-1:0] state = WAIT_ON_ARM;
|
||||||
|
|
||||||
|
/***** Outline *****
|
||||||
|
* The loop will only iterate when armed. If it is running and `arm`
|
||||||
|
* is deasserted, then it will complete the iteration it is in and
|
||||||
|
* stop.
|
||||||
|
*
|
||||||
|
* At the start of each loop, the constants are read into registers,
|
||||||
|
* so the constants can change while the loop is running.
|
||||||
|
*
|
||||||
|
* First the loop reads from the ADC, and then computes the error
|
||||||
|
* from the setpoint. The setpoint is specified in the same units
|
||||||
|
* as the ADC.
|
||||||
|
*
|
||||||
|
* Afterwards the loop computes the multiplications in the PI loop.
|
||||||
|
* This changes the size of the values in the loop.
|
||||||
|
*
|
||||||
|
* Combinatorially, the new adjusted value is calculated. The original
|
||||||
|
* value has to be stored in the same width as the multiplied values.
|
||||||
|
*
|
||||||
|
* Then the value is truncated to the width of the DAC, with saturation
|
||||||
|
* if necessary, and written out to the DAC.
|
||||||
|
*
|
||||||
|
**** Precision Propogation
|
||||||
|
*
|
||||||
|
* Measured value: ADC_WID.0
|
||||||
|
* Setpoint: ADC_WID.0
|
||||||
|
* - ----------------------------|
|
||||||
|
* e: ERR_WID.0
|
||||||
|
*
|
||||||
|
* α: CONSTS_WHOLE.CONSTS_FRAC | P: CONSTS_WHOLE.CONSTS_FRAC
|
||||||
|
* e: ERR_WID.0 | e_p: ERR_WID.0
|
||||||
|
* x ----------------------------| x-----------------------------
|
||||||
|
* αe: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC - Pe_p: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC
|
||||||
|
* + A_p: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC
|
||||||
|
* --------------------------------------------------------------
|
||||||
|
* A_p + αe - Pe_p: CONSTS_WHOLE+ERR_WID+1.CONSTS_FRAC
|
||||||
|
* --> discard fractional bits: CONSTS_WHOLE+ADC_WID+1.(DAC_DATA_WID - ADC_WID)
|
||||||
|
* --> Saturate-truncate: ADC_WID.(DAC_DATA_WID-ADC_WID)
|
||||||
|
* --> reinterpret and write into DAC: DAC_DATA_WID.0
|
||||||
|
*/
|
||||||
|
|
||||||
|
/******* Get measured value ********/
|
||||||
|
|
||||||
|
reg signed [ADC_WID-1:0] measured = 0;
|
||||||
|
reg adc_arm = 0;
|
||||||
|
wire adc_finished;
|
||||||
|
spi_master_no_write #(
|
||||||
|
.WID(ADC_WID),
|
||||||
|
.POLARITY(ADC_POLARITY),
|
||||||
|
.PHASE(ADC_PHASE),
|
||||||
|
.CYCLE_HALF_WAIT(1),
|
||||||
|
.TIMER_LEN(3)
|
||||||
|
) adc (
|
||||||
|
.clk(clk),
|
||||||
|
.from_slave(adcbuf),
|
||||||
|
.miso(adc_in),
|
||||||
|
.sck_wire(adc_sck),
|
||||||
|
.arm(adc_arm),
|
||||||
|
.finished(adc_finished)
|
||||||
|
);
|
||||||
|
|
||||||
|
/**** Calculate Error ****/
|
||||||
|
assign err_cur = measured - setpoint;
|
||||||
|
|
||||||
|
/****** Multiplication *******
|
||||||
|
* Truncation of a fixed-point integer to a smaller buffer requires
|
||||||
|
* 1) truncating higher order bits
|
||||||
|
* 2) removing lower order bits
|
||||||
|
*
|
||||||
|
* The ADC number has no fractional digits, so the fixed point output
|
||||||
|
* is [CONSTS_WHOLE + ERR_WID].CONSTS_FRAC_WID
|
||||||
|
* with total width CONSTS_WID + ERR_WID
|
||||||
|
*
|
||||||
|
* Both multipliers are armed at the same time.
|
||||||
|
* Their output wires are ANDed together so the state machine
|
||||||
|
* progresses when both are finished.
|
||||||
|
*/
|
||||||
|
|
||||||
|
localparam MUL_WHOLE_WID = CONSTS_WHOLE + ERR_WID;
|
||||||
|
localparam MUL_FRAC_WID = CONSTS_FRAC;
|
||||||
|
localparam MUL_WID = MUL_WHOLE_WID + MUL_FRAC_WID;
|
||||||
|
|
||||||
|
reg arm_mul = 0;
|
||||||
|
|
||||||
|
wire alpha_err_fin;
|
||||||
|
wire signed [MUL_WID-1:0] alpha_err;
|
||||||
|
wire p_err_prev_fin;
|
||||||
|
wire signed [MUL_WID-1:0] p_err_prev;
|
||||||
|
|
||||||
|
wire mul_finished = alpha_err_prev_fin & p_err_fin;
|
||||||
|
|
||||||
|
/* αe */
|
||||||
|
boothmul #(
|
||||||
|
.A1_LEN(CONSTS_WID),
|
||||||
|
.A2_LEN(ERR_WID),
|
||||||
|
.A2LEN_SIZ(ERR_WID_SIZ)
|
||||||
|
) boothmul_alpha_err_mul (
|
||||||
|
.clk(clk),
|
||||||
|
.arm(arm_mul),
|
||||||
|
.a1(cl_alpha_reg),
|
||||||
|
.a2(err),
|
||||||
|
.outn(alpha_err),
|
||||||
|
.fin(alpha_err_fin)
|
||||||
|
);
|
||||||
|
|
||||||
|
/* Pe_p */
|
||||||
|
boothmul #(
|
||||||
|
.A1_LEN(CONSTS_WID),
|
||||||
|
.A2_LEN(ERR_WID),
|
||||||
|
.A2LEN_SIZ(ERR_WID_SIZ)
|
||||||
|
) booth_mul_P_err_mul (
|
||||||
|
.clk(clk),
|
||||||
|
.arm(arm_mul),
|
||||||
|
.a1(cl_p_reg),
|
||||||
|
.a2(err_prev),
|
||||||
|
.outn(p_err_prev),
|
||||||
|
.fin(p_err_prev_fin)
|
||||||
|
);
|
||||||
|
|
||||||
|
/**** Subtraction after multiplication ****/
|
||||||
|
localparam SUB_WHOLE_WID = MUL_WHOLE_WID + 1;
|
||||||
|
localparam SUB_FRAC_WID = MUL_FRAC_WID;
|
||||||
|
localparam SUB_WID = SUB_WHOLE_WID + SUB_FRAC_WID;
|
||||||
|
|
||||||
|
reg signed [SUB_WID-1:0] adj_old;
|
||||||
|
wire signed [SUB_WID-1:0] newadj = adj_old + alpha_err - p_err_prev;
|
||||||
|
|
||||||
|
/**** Discard fractional bits ****
|
||||||
|
* The truncation of the subtraction result first takes off the lower
|
||||||
|
* order bits:
|
||||||
|
* [ SUB_WHOLE_WID ].[ SUB_FRAC_WID ]
|
||||||
|
* [ SUB_WHOLE_WID ].[RTRUNC_FRAC_WID]############
|
||||||
|
* (SUB_FRAC_WID - RTRUNC_FRAC_WID)^
|
||||||
|
*/
|
||||||
|
|
||||||
|
localparam RTRUNC_FRAC_WID = DAC_DATA_WID - ADC_WID;
|
||||||
|
localparam RTRUNC_WHOLE_WID = SUB_WHOLE_WID;
|
||||||
|
localparam RTRUNC_WID = RTRUNC_WHOLE_WID + RTRUNC_FRAC_WID;
|
||||||
|
|
||||||
|
wire signed rtrunc[RTRUNC_WID-1:0] =
|
||||||
|
newadj[SUB_WID-1:SUB_FRAC_WID-RTRUNC_FRAC_WID];
|
||||||
|
|
||||||
|
/**** Truncate-Saturate ****
|
||||||
|
* Truncate the result into a value acceptable to the DAC.
|
||||||
|
* [ SUB_WHOLE_WID ].[RTRUNC_FRAC_WID]############
|
||||||
|
* [ADC_WID].[DAC_DATA_WID - ADC_WID]
|
||||||
|
* reinterpreted as
|
||||||
|
* [DAC_DATA_WID].0
|
||||||
|
*/
|
||||||
|
|
||||||
|
wire signed dac_adj_val[DAC_DATA_WID-1:0];
|
||||||
|
|
||||||
|
intsat #(
|
||||||
|
.IN_LEN(RTRUNC_WID),
|
||||||
|
.LTRUNC(DAC_DATA_WID)
|
||||||
|
) (
|
||||||
|
.inp(newadj_rtrunc),
|
||||||
|
.outp(dac_adj_val)
|
||||||
|
);
|
||||||
|
|
||||||
|
/**** Write to DAC ****/
|
||||||
|
|
||||||
|
reg dac_arm = 0;
|
||||||
|
spi_master_no_read #(
|
||||||
|
.WID(DAC_WID),
|
||||||
|
.WID_LEN(5),
|
||||||
|
.CYCLE_HALF_WAIT(3),
|
||||||
|
.TIMER_LEN(3),
|
||||||
|
.POLARITY(DAC_POLARITY),
|
||||||
|
.PHASE(DAC_PHASE)
|
||||||
|
) dac (
|
||||||
|
.clk(clk),
|
||||||
|
.to_slave({4'b0010,dac_adj_val}),
|
||||||
|
.mosi(dac_out),
|
||||||
|
.sck_wire(dac_sck),
|
||||||
|
.arm(dac_arm)
|
||||||
|
);
|
||||||
|
|
||||||
|
reg [DELAY_WID-1:0] timer = 0;
|
||||||
|
|
||||||
|
always @ (posedge clk) begin
|
||||||
|
case (state)
|
||||||
|
WAIT_ON_ARM: begin
|
||||||
|
if (!arm) begin
|
||||||
|
adj_prev <= 0;
|
||||||
|
err_prev <= 0;
|
||||||
|
timer <= 0;
|
||||||
|
end else if (timer == 0) begin
|
||||||
|
saved_delay <= delay_in;
|
||||||
|
timer <= 1;
|
||||||
|
end else if (timer < saved_delay) begin
|
||||||
|
timer <= timer + 1;
|
||||||
|
setpt <= setpt_in;
|
||||||
|
cl_alpha_reg <= cl_alpha_in;
|
||||||
|
cl_p_reg <= cl_p_in;
|
||||||
|
end else begin
|
||||||
|
state <= WAIT_ON_ADC;
|
||||||
|
timer <= 0;
|
||||||
|
adc_arm <= 1;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
WAIT_ON_ADC: if (adc_finished) begin
|
||||||
|
adc_arm <= 0;
|
||||||
|
arm_mul <= 1;
|
||||||
|
state <= WAIT_ON_MUL;
|
||||||
|
end
|
||||||
|
WAIT_ON_MUL: if (mul_finished) begin
|
||||||
|
arm_mul <= 0;
|
||||||
|
dac_arm <= 1;
|
||||||
|
state <= WAIT_ON_DAC;
|
||||||
|
end
|
||||||
|
WAIT_ON_DAC: if (dac_finished) begin
|
||||||
|
state <= WAIT_ON_ARM;
|
||||||
|
dac_arm <= 0;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
endmodule
|
||||||
|
|
@ -0,0 +1,45 @@
|
||||||
|
/* Saturate integers. v0.1
|
||||||
|
* Written by Peter McGoron, 2022.
|
||||||
|
*/
|
||||||
|
|
||||||
|
module intsat
|
||||||
|
#(
|
||||||
|
parameter IN_LEN = 64,
|
||||||
|
parameter LTRUNC = 32
|
||||||
|
)
|
||||||
|
(
|
||||||
|
input signed [IN_LEN-1:0] inp,
|
||||||
|
output signed [IN_LEN-LTRUNC-1:0] outp
|
||||||
|
);
|
||||||
|
|
||||||
|
/* Truncating a twos-complement integer can cause overflow.
|
||||||
|
* To check for overflow, look at all truncated bits and
|
||||||
|
* the most significant bit that will be kept.
|
||||||
|
* If they are all 0 or all 1, then there is no truncation.
|
||||||
|
*/
|
||||||
|
|
||||||
|
localparam INP_TRUNC_START = IN_LEN - LTRUNC;
|
||||||
|
localparam INP_CHECK_START = INP_TRUNC_START - 1;
|
||||||
|
localparam OUT_LEN = IN_LEN - LTRUNC;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* [ IN_LEN ]
|
||||||
|
* [ LTRUNC | OUT_LEN ]
|
||||||
|
* * &
|
||||||
|
* *: INP_TRUNC_START
|
||||||
|
* &: INP_CHECK_START
|
||||||
|
*/
|
||||||
|
|
||||||
|
always @ (*) begin
|
||||||
|
if (inp[IN_LEN-1:INP_CHECK_START] == {(LTRUNC + 1){inp[IN_LEN-1]}}) begin
|
||||||
|
outp[OUT_LEN-1:0] = inp[OUT_LEN-1:0];
|
||||||
|
end else if (inp[IN_LEN-1]) begin
|
||||||
|
// most negative number: 1000000....
|
||||||
|
outp[OUT_LEN-1:0] = {1'b1,{(OUT_LEN-1){1'b0}}};
|
||||||
|
end else begin
|
||||||
|
// most positive number: 0111111....
|
||||||
|
outp[OUT_LEN-1:0] = {1'b0,{(OUT_LEN-1){1'b1}}};
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
endmodule
|
||||||
|
|
@ -1,7 +1,5 @@
|
||||||
/* (c) Peter McGoron 2022
|
/* SPI master.
|
||||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
* Written by Peter McGoron, 2022.
|
||||||
* License, v.2.0. If a copy of the MPL was not distributed with this
|
|
||||||
* file, You can obtain one at https://mozilla.org/MPL/2.0/.
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
module
|
module
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,3 @@
|
||||||
|
`define SPI_MASTER_NO_READ
|
||||||
|
/* verilator lint_off DECLFILENAME */
|
||||||
|
`include "spi_master.v"
|
||||||
|
|
@ -0,0 +1,76 @@
|
||||||
|
#include <memory>
|
||||||
|
#include <limits>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <iostream>
|
||||||
|
#include <verilated.h>
|
||||||
|
#include "Vboothmul.h"
|
||||||
|
|
||||||
|
using word = int16_t;
|
||||||
|
using dword = int32_t;
|
||||||
|
|
||||||
|
constexpr word minint = std::numeric_limits<word>::min();
|
||||||
|
constexpr word maxint = std::numeric_limits<word>::max();
|
||||||
|
|
||||||
|
uint32_t main_time = 0;
|
||||||
|
|
||||||
|
double sc_time_stamp() {
|
||||||
|
return main_time;
|
||||||
|
}
|
||||||
|
|
||||||
|
Vboothmul *mod;
|
||||||
|
|
||||||
|
static void run_clock() {
|
||||||
|
mod->clk = !mod->clk;
|
||||||
|
mod->eval();
|
||||||
|
main_time++;
|
||||||
|
|
||||||
|
mod->clk = !mod->clk;
|
||||||
|
mod->eval();
|
||||||
|
main_time++;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void run(word i, word j) {
|
||||||
|
// Processor is twos-compliment
|
||||||
|
mod->a1 = i;
|
||||||
|
mod->a2 = j;
|
||||||
|
mod->arm = 1;
|
||||||
|
|
||||||
|
while (!mod->fin)
|
||||||
|
run_clock();
|
||||||
|
|
||||||
|
dword expected = (dword) i * (dword) j;
|
||||||
|
if (mod->outn != expected) {
|
||||||
|
std::cout << i << "*" << j << "=" << expected
|
||||||
|
<< "(" << mod->outn << ")" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
mod->arm = 0;
|
||||||
|
run_clock();
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char **argv) {
|
||||||
|
Verilated::commandArgs(argc, argv);
|
||||||
|
// Verilated::traceEverOn(true);
|
||||||
|
mod = new Vboothmul;
|
||||||
|
|
||||||
|
mod->clk = 0;
|
||||||
|
mod->arm = 0;
|
||||||
|
run_clock();
|
||||||
|
|
||||||
|
run(minint, minint);
|
||||||
|
run(minint, maxint);
|
||||||
|
run(maxint, minint);
|
||||||
|
run(maxint, maxint);
|
||||||
|
|
||||||
|
for (word i = -20; i < 20; i++) {
|
||||||
|
for (word j = - 20; j < 20; j++) {
|
||||||
|
run(i, j);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
mod->final();
|
||||||
|
|
||||||
|
delete mod;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
#include <memory>
|
||||||
|
#include <limits>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <iostream>
|
||||||
|
#include <random>
|
||||||
|
|
||||||
|
#include <verilated.h>
|
||||||
|
#include "Vcontrol_loop.h"
|
||||||
|
|
||||||
|
Vcontrol_loop *mod;
|
||||||
|
|
||||||
|
/* Very simple simulation of measurement.
|
||||||
|
* A transfer function defines the mapping from the DAC values
|
||||||
|
* -2**(20) -> 2**(20)-1
|
||||||
|
* to the values -2**(18) -> 2**18 - 1.
|
||||||
|
*
|
||||||
|
* The transfer function has Gaussian noise which is added at each
|
||||||
|
* measurement.
|
||||||
|
*/
|
||||||
|
|
||||||
|
class Transfer {
|
||||||
|
std::random_device rd;
|
||||||
|
std::normal_distribution dist;
|
||||||
|
double scale;
|
||||||
|
|
||||||
|
double sample() {return scale*dist(rd);}
|
||||||
|
|
||||||
|
public:
|
||||||
|
Transfer(double scale, double mean, double dev, double m, double b)
|
||||||
|
: scale{scale}, rd{}, dist{mean,dev} {}
|
||||||
|
|
||||||
|
double val(double x) {
|
||||||
|
return m*x + b + sample();
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
int main(int argc, char **argv) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,42 @@
|
||||||
|
module top
|
||||||
|
#(
|
||||||
|
parameter ADC_WID = 18,
|
||||||
|
parameter DAC_WID = 24,
|
||||||
|
parameter ADC_POLARITY = 1,
|
||||||
|
parameter ADC_PHASE = 0,
|
||||||
|
parameter DAC_DATA_WID = 20,
|
||||||
|
parameter CONSTS_WID = 48,
|
||||||
|
parameter DELAY_WID = 16
|
||||||
|
(
|
||||||
|
input clk,
|
||||||
|
input signed [ADC_WID-1:0] read_data,
|
||||||
|
output signed [DAC_WID-1:0] write_data,
|
||||||
|
input signed [ADC_WID-1:0] setpt,
|
||||||
|
input signed [CONSTS_WID-1:0] alpha,
|
||||||
|
input signed [CONSTS_WID-1:0] cl_p,
|
||||||
|
input [DELAY_WID-1:0] dy,
|
||||||
|
|
||||||
|
output signed [ADC_WID:0] err,
|
||||||
|
output signed [CONSTS_WID-1:0] adj
|
||||||
|
);
|
||||||
|
|
||||||
|
wire adc_sck;
|
||||||
|
wire adc_ss;
|
||||||
|
wire adc_mosi;
|
||||||
|
|
||||||
|
spi_slave_no_write #(
|
||||||
|
.WID(ADC_WID),
|
||||||
|
.WID(5),
|
||||||
|
.
|
||||||
|
|
||||||
|
control_loop #(
|
||||||
|
.ADC_WID(ADC_WID),
|
||||||
|
.DAC_WID(DAC_WID),
|
||||||
|
.DAC_DATA_WID(DAC_DATA_WID),
|
||||||
|
.CONSTS_WID(CONSTS_WID),
|
||||||
|
.DELAY_WID(DELAY_WID)
|
||||||
|
) cloop (
|
||||||
|
|
||||||
|
);
|
||||||
|
|
||||||
|
endmodule
|
||||||
|
|
@ -0,0 +1,55 @@
|
||||||
|
#include <memory>
|
||||||
|
#include <limits>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <iostream>
|
||||||
|
#include <verilated.h>
|
||||||
|
#include "Vintsat.h"
|
||||||
|
|
||||||
|
using dword = int16_t;
|
||||||
|
using word = int8_t;
|
||||||
|
|
||||||
|
double sc_time_stamp() {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
Vintsat *mod;
|
||||||
|
|
||||||
|
static void
|
||||||
|
run(dword i)
|
||||||
|
{
|
||||||
|
const auto max = std::numeric_limits<word>::max();
|
||||||
|
const auto min = std::numeric_limits<word>::min();
|
||||||
|
mod->inp = i;
|
||||||
|
mod->eval();
|
||||||
|
|
||||||
|
int in = (dword) mod->inp;
|
||||||
|
int out = (word) mod->outp;
|
||||||
|
|
||||||
|
if (i <= max && i >= min) {
|
||||||
|
if (in != out)
|
||||||
|
std::cout << in << "->" << out << std::endl;
|
||||||
|
} else {
|
||||||
|
if (i < min && out != min)
|
||||||
|
std::cout << in << "->" << out << std::endl;
|
||||||
|
else if (i > max && out != max)
|
||||||
|
std::cout << in << "->" << out << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char **argv) {
|
||||||
|
Verilated::commandArgs(argc, argv);
|
||||||
|
mod = new Vintsat;
|
||||||
|
|
||||||
|
dword i;
|
||||||
|
for (i = std::numeric_limits<dword>::min();
|
||||||
|
i < std::numeric_limits<dword>::max();
|
||||||
|
i++)
|
||||||
|
run(i);
|
||||||
|
run(i);
|
||||||
|
|
||||||
|
mod->final();
|
||||||
|
|
||||||
|
delete mod;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,42 @@
|
||||||
|
K_SEM_DEFINE(dhcp_ready, 0, 1);
|
||||||
|
|
||||||
|
static bool
|
||||||
|
check_dhcp(struct net_if *iface)
|
||||||
|
{
|
||||||
|
// Scan IP addresses allocated for Unicast
|
||||||
|
for (int i = 0; i < NET_IF_MAX_IPV4_ADDR; i++) {
|
||||||
|
if (iface->config.ip.ipv4->unicast[i].addr_type != NET_ADDR_DHCP)
|
||||||
|
continue;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
dhcp_handler(struct net_mgmt_event_callback *cb,
|
||||||
|
uint32_t ev,
|
||||||
|
struct net_if *iface)
|
||||||
|
{
|
||||||
|
if (ev != NET_EVENT_IPV4_ADDR_ADD)
|
||||||
|
return;
|
||||||
|
if (check_dhcp(iface))
|
||||||
|
k_sem_give(&dhcp_ready);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
setup_dhcp(void)
|
||||||
|
{
|
||||||
|
|
||||||
|
static struct net_mgmt_event_callback cb;
|
||||||
|
net_mgmt_init_event_callback(&cb, dhcp_handler, NET_EVENT_IPV4_ADDR_ADD);
|
||||||
|
net_mgmt_add_event_callback(&cb);
|
||||||
|
|
||||||
|
struct net_if *iface = net_if_get_default();
|
||||||
|
|
||||||
|
if (!check_dhcp(iface)) {
|
||||||
|
net_dhcpv4_start(iface);
|
||||||
|
k_sem_take(&dhcp_ready, K_FOREVER);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,97 @@
|
||||||
|
#include <zephyr/zephyr.h>
|
||||||
|
#include <zephyr/sys/printk.h>
|
||||||
|
#include <zephyr/net/socket.h>
|
||||||
|
#include "buf.h"
|
||||||
|
|
||||||
|
/* Read from the socket into the buffer.
|
||||||
|
* This function is meant to be called multiple times on the
|
||||||
|
* same struct. The controller loads bp->left with the amount
|
||||||
|
* of bytes it wishes to read, and continues until bp->left == 0.
|
||||||
|
*
|
||||||
|
* This function returns false if there was an error reading
|
||||||
|
* from the socket.
|
||||||
|
* A read of 0 bytes returns true.
|
||||||
|
*/
|
||||||
|
bool
|
||||||
|
buf_read_sock(int sock, struct bufptr *bp)
|
||||||
|
{
|
||||||
|
ssize_t l = zsock_recv(sock, bp->p, bp->left, 0);
|
||||||
|
if (l < 0)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
bp->left -= l;
|
||||||
|
bp->p += l;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Write from the bufptr into a socket.
|
||||||
|
* This function is meant to be called once per prepared bufptr.
|
||||||
|
*
|
||||||
|
* This function returns false if there was an error on the
|
||||||
|
* socket.
|
||||||
|
*/
|
||||||
|
bool
|
||||||
|
buf_write_sock(int sock, struct bufptr *bp)
|
||||||
|
{
|
||||||
|
while (bp->left) {
|
||||||
|
ssize_t l = zsock_send(sock, bp->p, bp->left, 0);
|
||||||
|
if (l < 0)
|
||||||
|
return false;
|
||||||
|
bp->p += l;
|
||||||
|
bp->left -= l;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Write a formatted string to bp.
|
||||||
|
* This function uses printf(), which means that it deals with
|
||||||
|
* writing _C-strings_, not unterminated buffers.
|
||||||
|
* When using this function, the buffer must be _one more_ than
|
||||||
|
* the maximum message length. For instance, a 1024-byte message
|
||||||
|
* should be in a 1025-byte buffer. HOWEVER, bp->left must still
|
||||||
|
* be set to the total length of the buffer (in the example, 1025).
|
||||||
|
*
|
||||||
|
* The final bufptr points to the NUL terminator, so that it
|
||||||
|
* is overwritten on each call to the function.
|
||||||
|
*
|
||||||
|
* This function returns 0 for a successful write, -1 for an
|
||||||
|
* encoding error (should never happen), and a positive value
|
||||||
|
* for the amount of bytes that could not fit.
|
||||||
|
*/
|
||||||
|
int
|
||||||
|
buf_writevf(struct bufptr *bp, const char *fmt, va_list va)
|
||||||
|
{
|
||||||
|
int w;
|
||||||
|
|
||||||
|
/* vsnprintk() returns the amount of bytes that would
|
||||||
|
* be stored in the buffer if the buffer was big enough,
|
||||||
|
* excluding the NUL terminator.
|
||||||
|
* The function will _always_ write a NUL terminator
|
||||||
|
* unless bp->left == 0.
|
||||||
|
*/
|
||||||
|
w = vsnprintk(bp->p, bp->left, fmt, va);
|
||||||
|
|
||||||
|
if (w < 0)
|
||||||
|
return BUF_WRITE_ERR;
|
||||||
|
|
||||||
|
if (w >= bp->left) {
|
||||||
|
size_t oldleft = bp->left;
|
||||||
|
buf->p += bp->left - 1;
|
||||||
|
bp->left = 1;
|
||||||
|
return w - oldleft + 1;
|
||||||
|
} else {
|
||||||
|
bp->p += w;
|
||||||
|
bp->left -= w;
|
||||||
|
return BUF_OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
buf_writef(struct bufptr *bp, const char *fmt, ...)
|
||||||
|
{
|
||||||
|
va_list va;
|
||||||
|
va_start(va, fmt);
|
||||||
|
buf_writevf(bp, fmt, va);
|
||||||
|
va_end(va);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
/* This is a pointer _into_ a buffer. It is increment
|
||||||
|
* (and the variable left decremented) after each
|
||||||
|
* operation.
|
||||||
|
*/
|
||||||
|
struct bufptr {
|
||||||
|
char *p;
|
||||||
|
size_t left;
|
||||||
|
};
|
||||||
|
|
||||||
|
enum {
|
||||||
|
BUF_OK = 0,
|
||||||
|
BUF_WRITE_ERR = -1
|
||||||
|
};
|
||||||
|
|
||||||
|
bool buf_read_sock(int sock, struct bufptr *bp);
|
||||||
|
bool buf_write_sock(int sock, struct bufptr *bp);
|
||||||
|
int buf_writevf(struct bufptr *bp, const char *fmt, va_list va);
|
||||||
|
int buf_writef(struct bufptr *bp, const char *fmt, ...);
|
||||||
|
|
@ -1,4 +1,35 @@
|
||||||
/* From: https://github.com/zserge/jsmn 25647e692c7906b96ffd2b05ca54c097948e879c
|
/*
|
||||||
|
* Jsmn is JSON parser that does not use the C standard library and
|
||||||
|
* does not allocate memory. Jsmn stores the structure of the JSON
|
||||||
|
* document in an array that records
|
||||||
|
* * the type of the token,
|
||||||
|
* * the start position and extent of the token,
|
||||||
|
* * and the number of elements inside the token.
|
||||||
|
* For example, the document
|
||||||
|
{"key":"val", "obj" : { "x" : 2, "y" : false }, "arr" : [1, "2", 3]}
|
||||||
|
* would contain parse to:
|
||||||
|
* OBJECT 6, STRING "key", STRING "val", STRING "obj", OBJECT 4,
|
||||||
|
* STRING "key", PRIMITIVE 2, STRING y, PRIMITIVE false,
|
||||||
|
* STRING "arr", ARRAY 3, PRIMITIVE 1, STRING "2", PRIMITIVE 3
|
||||||
|
* The number next to OBJECT and ARRAY denote the size of each.
|
||||||
|
* Strings always start with a quote and end with one: primitives
|
||||||
|
* (numbers, booleans, null) do not.
|
||||||
|
*
|
||||||
|
* TOKENS ARE NOT NUL TERMINATED. However, because of how the JSON
|
||||||
|
* grammar works, the NUL terminator can be manually added.
|
||||||
|
* STRING ESCAPE SEQUENCES ARE NOT PARSED.
|
||||||
|
* Strings start at one-past the quotation mark, and the end index
|
||||||
|
* points to the end quotation mark.
|
||||||
|
*
|
||||||
|
* Why JSON?
|
||||||
|
* 1) Every language has a JSON library.
|
||||||
|
* 2) JSMN is perfectly suited to this program.
|
||||||
|
* 3) Another standard format would most likely require writing
|
||||||
|
* a parser by hand.
|
||||||
|
* 4) A minimalistic, custom format would require more debugging.
|
||||||
|
*
|
||||||
|
* From: https://github.com/zserge/jsmn 25647e692c7906b96ffd2b05ca54c097948e879c
|
||||||
|
*
|
||||||
* MIT License
|
* MIT License
|
||||||
*
|
*
|
||||||
* Copyright (c) 2010 Serge Zaitsev
|
* Copyright (c) 2010 Serge Zaitsev
|
||||||
|
|
|
||||||
|
|
@ -1,26 +1,58 @@
|
||||||
#include <zephyr/zephyr.h>
|
#include <zephyr/zephyr.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <zephyr/net/socket.h>
|
||||||
#include <zephyr/kernel.h>
|
#include <zephyr/kernel.h>
|
||||||
#include <zephyr/logging/log.h>
|
#include <zephyr/logging/log.h>
|
||||||
#include "pin_io.h"
|
|
||||||
|
#include "sock.h"
|
||||||
|
|
||||||
|
LOG_MODULE_REGISTER(main);
|
||||||
|
|
||||||
|
#define PORT 6626
|
||||||
|
enum fds {
|
||||||
|
CLIENT_FD,
|
||||||
|
SCANDATA_FD,
|
||||||
|
MAXFDS
|
||||||
|
};
|
||||||
|
|
||||||
|
static void
|
||||||
|
process_client(int cli)
|
||||||
|
{
|
||||||
|
struct zsock_pollfd fds[MAXFDS] = {0};
|
||||||
|
struct clireadbuf readbuf = {0};
|
||||||
|
|
||||||
|
client_buf_reset(&readbuf);
|
||||||
|
|
||||||
|
fds[CLIENT_FD].fd = cli;
|
||||||
|
fds[CLIENT_FD].events = ZSOCK_POLLIN;
|
||||||
|
// Currently not used
|
||||||
|
fds[SCANDATA_FD].fd = -1;
|
||||||
|
|
||||||
|
while (zsock_poll(fds, MAXFDS, 0) >= 0) {
|
||||||
|
if (fds[CLIENT_FD].revents | POLLIN) {
|
||||||
|
if (!client_read_into_buf(cli, &readbuf)) {
|
||||||
|
INFO_WRN("client_read_into_buf: %d", errno);
|
||||||
|
goto cleanup;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (readbuf.st == MSG_READY) {
|
||||||
|
msg_parse_dispatch(cli, &readbuf);
|
||||||
|
client_buf_reset(&buf);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cleanup:
|
||||||
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
main(void)
|
main(void)
|
||||||
{
|
{
|
||||||
uint32_t v = 0;
|
int srv = server_init_sock(PORT);
|
||||||
uint32_t r = 0;
|
|
||||||
LOG_PRINTK("hello, world\n");
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
k_sleep(K_MSEC(1000));
|
int cli = server_get_client(server_sock);
|
||||||
#if 0 // ADC code
|
process_client(cli);
|
||||||
*adc_conv[0] = v;
|
LOG_INF("Closing client socket");
|
||||||
v = !v;
|
zsock_close(cli);
|
||||||
r = *adc_sdo[0];
|
|
||||||
#endif
|
|
||||||
*dac_ctrl[0] = v;
|
|
||||||
v++;
|
|
||||||
if (v == 8)
|
|
||||||
v = 0;
|
|
||||||
r = *dac_miso[0];
|
|
||||||
LOG_PRINTK("out: %d; in: %d\n", v, r);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,211 @@
|
||||||
|
#include <zephyr.h>
|
||||||
|
#include <zephyr/net/socket.h>
|
||||||
|
#include <zephyr/kernel.h>
|
||||||
|
#include <zephyr/logging/log.h>
|
||||||
|
#include <stdarg.h>
|
||||||
|
|
||||||
|
#define JSMN_PARENT_LINKS
|
||||||
|
#define JSMN_STRICT
|
||||||
|
#define JSMN_STATIC
|
||||||
|
#include "jsmn.h"
|
||||||
|
#include "sock.h"
|
||||||
|
#include "buf.h"
|
||||||
|
|
||||||
|
LOG_MODULE_REGISTER(msg);
|
||||||
|
|
||||||
|
enum cmdtype {
|
||||||
|
NONE,
|
||||||
|
IDENT,
|
||||||
|
CONSTS,
|
||||||
|
RAMP,
|
||||||
|
READ_ADC,
|
||||||
|
RESET_DAC,
|
||||||
|
RESET,
|
||||||
|
CMDTYPE_LEN
|
||||||
|
};
|
||||||
|
|
||||||
|
struct cmd {
|
||||||
|
enum cmdtype typ;
|
||||||
|
char *version;
|
||||||
|
char *msgid;
|
||||||
|
char *emsg;
|
||||||
|
union {
|
||||||
|
char *ident;
|
||||||
|
struct {
|
||||||
|
int P;
|
||||||
|
int I;
|
||||||
|
int Vnm;
|
||||||
|
} consts;
|
||||||
|
struct {
|
||||||
|
int dac;
|
||||||
|
int offset;
|
||||||
|
int dely;
|
||||||
|
} rampcmd;
|
||||||
|
int read_adc;
|
||||||
|
int reset_dac;
|
||||||
|
struct {
|
||||||
|
int id;
|
||||||
|
int blklen;
|
||||||
|
int siz;
|
||||||
|
} startscan;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
|
||||||
|
struct parser {
|
||||||
|
struct readbuf *js; // JSON buffer.
|
||||||
|
jsmntok_t *ctok; // Parser is at this token.
|
||||||
|
jsmntok_t *last; // Last token in the sequence.
|
||||||
|
int cli; // Socket.
|
||||||
|
struct cmd cmd; // Command parsed into.
|
||||||
|
};
|
||||||
|
|
||||||
|
enum {
|
||||||
|
OK = 0,
|
||||||
|
CONVERR = BUF_WRITE_ERR,
|
||||||
|
SOCKERR = BUF_WRITE_ERR - 1
|
||||||
|
};
|
||||||
|
|
||||||
|
// Serialize struct cmd into JSON and send it.
|
||||||
|
static int
|
||||||
|
dispatch(int sock, struct cmd *cmd)
|
||||||
|
{
|
||||||
|
// Add byte for NUL terminator for snprintf()
|
||||||
|
char buf[CLIREADBUF_SIZ + sizeof(uint16_t) + 1] = {0};
|
||||||
|
struct bufptr bp = {buf, sizeof(buf)};
|
||||||
|
int r;
|
||||||
|
|
||||||
|
/* If no version could be read (i.e. sending a message
|
||||||
|
* saying that the JSON was invalid) then send a
|
||||||
|
* version 0 message.
|
||||||
|
*/
|
||||||
|
if (!cmd->version)
|
||||||
|
cmd->version = "0";
|
||||||
|
|
||||||
|
if ((r = buf_writef(&bp, "{\"version\":\"%s\"", cmd->version)) != BUF_OK)
|
||||||
|
return r;
|
||||||
|
|
||||||
|
if (cmd->msgid) {
|
||||||
|
if ((r = buf_writef(&bp, ",\"msgid\":\"%s\"", cmd->msgid)) != BUF_OK)
|
||||||
|
return r;
|
||||||
|
}
|
||||||
|
if (cmd->emsg) {
|
||||||
|
if ((r = buf_writef(&bp, ",\"error\":\"%s\", cmd->emsg)) != BUF_OK)
|
||||||
|
return r;
|
||||||
|
goto send;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!cmd->emsg) switch (cmd->typ) {
|
||||||
|
case IDENT:
|
||||||
|
if ((r = buf_writef(&bp, ",\"%s\":\"%s\", sl[IDENT].s, "cryosnom") != 0)
|
||||||
|
return r;
|
||||||
|
goto send;
|
||||||
|
case CONSTS:
|
||||||
|
// STUB
|
||||||
|
goto send;
|
||||||
|
case RAMPCMD:
|
||||||
|
// STUB
|
||||||
|
goto send;
|
||||||
|
case READ_ADC:
|
||||||
|
// STUB
|
||||||
|
goto send;
|
||||||
|
case READ_DAC:
|
||||||
|
// STUB
|
||||||
|
goto send;
|
||||||
|
case STARTSCAN:
|
||||||
|
// STUB
|
||||||
|
goto send;
|
||||||
|
}
|
||||||
|
|
||||||
|
send:
|
||||||
|
if ((r = buf_writef(&bp, "}")) != 0)
|
||||||
|
return r;
|
||||||
|
|
||||||
|
struct bufptr wptr = {buf, sizeof(buf) - bp.left};
|
||||||
|
if (!buf_write_sock(sock, &wptr))
|
||||||
|
return SOCKERR;
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline bool
|
||||||
|
eq(jsmntok_t *tk, char *buf, const char *s, size_t slen)
|
||||||
|
{
|
||||||
|
return (slen == buf->end - buf->start
|
||||||
|
&& strncasecmp(s, &jr->js[buf->start], slen) == 0);
|
||||||
|
}
|
||||||
|
#define liteq(buf,tok,st) liteq((buf), (tok), (st), sizeof(st) - 1)
|
||||||
|
|
||||||
|
static inline void
|
||||||
|
jsmn_term(char *buf, jsmntok_t *tok)
|
||||||
|
{
|
||||||
|
buf[tok->end] = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static jsmntok_t
|
||||||
|
parse_consts(int sock, jsmntok_t *ct, int len, char *js)
|
||||||
|
{
|
||||||
|
while (len > 0) {
|
||||||
|
if (liteq(ct, js, "P")) {
|
||||||
|
ct++;
|
||||||
|
len--;
|
||||||
|
if (
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool
|
||||||
|
parse(int sock, jsmntok_t *first, jsmntok_t *last, char *js)
|
||||||
|
{
|
||||||
|
if (first->type != JSMN_OBJECT) {
|
||||||
|
psr->obj.emsg = "malformed json (not an object)";
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (jsmntok_t *ct = first; ct < psr->last; ct++) {
|
||||||
|
if (liteq(ct, js, "version")) {
|
||||||
|
ct++;
|
||||||
|
if (!liteq(ct, js, "0")) {
|
||||||
|
psr->obj.emsg = "invalid version";
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
jsmn_term(js, ct);
|
||||||
|
psr->version = js + ct->start;
|
||||||
|
} else if (liteq(ct, js, "msgid")) {
|
||||||
|
ct++;
|
||||||
|
jsmn_term(js, ct);
|
||||||
|
psr->msgid = js + ct->start;
|
||||||
|
} else if (liteq(ct, js, "ident")) {
|
||||||
|
ct++;
|
||||||
|
psr->typ = IDENT;
|
||||||
|
} else if (liteq(ct, js, "consts")) {
|
||||||
|
ct++;
|
||||||
|
psr->typ = CONSTS;
|
||||||
|
if (ct->type == JSMN_OBJECT) {
|
||||||
|
if (!(ct = parse_consts(sock, ct+1, ct->size, js)))
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
fail:
|
||||||
|
return dispatch(&psr->obj);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Read a JSON message, parse it, execute it, and respond to it.
|
||||||
|
*/
|
||||||
|
bool
|
||||||
|
msg_parse_dispatch(int cli, struct readbuf *buf)
|
||||||
|
{
|
||||||
|
jsmn_parser psr;
|
||||||
|
jsmntok_t tokens[JSON_MAX_LEN];
|
||||||
|
|
||||||
|
struct cmd cmd = {
|
||||||
|
.typ = NONE
|
||||||
|
};
|
||||||
|
|
||||||
|
jsmn_init(&psr);
|
||||||
|
|
||||||
|
if (jsmn_parse(buf->buf, buf->readlen, &tokens, JSON_MAX_LEN) < 0) {
|
||||||
|
psr.emsg = "malformed json";
|
||||||
|
return dispatch(cli, &cmd);
|
||||||
|
}
|
||||||
|
|
||||||
|
return parse(cli, &tokens[0], &tokens[JSON_MAX_LEN-1], buf);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,54 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
/* Controller only supports JSON_MAX_LEN JSON tokens.
|
||||||
|
* A token is
|
||||||
|
* * the beginning of an object
|
||||||
|
* * A key
|
||||||
|
* * any value
|
||||||
|
* * the beginning of an array
|
||||||
|
|
||||||
|
Due to a particular quirk of JSMN, the numbers
|
||||||
|
can also be strings. Numbers MUST be integers.
|
||||||
|
msgid SHOULD be a small number.
|
||||||
|
|
||||||
|
Messages are formatted like
|
||||||
|
{
|
||||||
|
"version" : num, // required, set to 0
|
||||||
|
"msgid" : str, // optional
|
||||||
|
"error" : str, // only from controller, ignored from computer
|
||||||
|
|
||||||
|
// and zero to one of
|
||||||
|
"ident" : null,
|
||||||
|
"consts" : { // all are optional
|
||||||
|
"P" : num,
|
||||||
|
"I" : num,
|
||||||
|
"Vnm" : num
|
||||||
|
}, // or null
|
||||||
|
"ramp" : {
|
||||||
|
"dac" : num,
|
||||||
|
"off" : num,
|
||||||
|
"dly" : num
|
||||||
|
},
|
||||||
|
"read_adc" : num,
|
||||||
|
"reset_dac" : num,
|
||||||
|
"reset_all" : null,
|
||||||
|
"startscan" : null, // from computer
|
||||||
|
"startscan" : { // from controller
|
||||||
|
"id" : int,
|
||||||
|
"blksiz" : int,
|
||||||
|
"blklen" : int
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Both the controller and the computer read and write these messages.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define JSON_MAX_LEN 32
|
||||||
|
|
||||||
|
enum msg_ret {
|
||||||
|
MSG_OK,
|
||||||
|
MSG_BAD_JSON,
|
||||||
|
MSG_SOCKERR
|
||||||
|
};
|
||||||
|
|
||||||
|
enum msg_ret msg_parse_dispatch(int client, struct readbuf *buf);
|
||||||
|
|
@ -0,0 +1,93 @@
|
||||||
|
#include <zephyr/zephyr.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <zephyr/net/socket.h>
|
||||||
|
#include <zephyr/kernel.h>
|
||||||
|
#include <zephyr/logging/log.h>
|
||||||
|
|
||||||
|
LOG_MODULE_REGISTER(sock);
|
||||||
|
|
||||||
|
int
|
||||||
|
server_init_sock(int port)
|
||||||
|
{
|
||||||
|
int sock;
|
||||||
|
|
||||||
|
sock = zsock_socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
|
||||||
|
|
||||||
|
if (sock < 0) {
|
||||||
|
LOG_ERR("error: socket: %d", sock);
|
||||||
|
k_fatal_halt(K_ERR_KERNEL_PANIC);
|
||||||
|
}
|
||||||
|
|
||||||
|
struct sockaddr_in addr = {
|
||||||
|
.sin_family = AF_INET,
|
||||||
|
.sin_addr = {.s_addr = htonl(INADDR_ANY)},
|
||||||
|
.sin_port = htons(port)
|
||||||
|
};
|
||||||
|
|
||||||
|
if (zsock_bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||||
|
LOG_ERR("error: bind: %d", errno);
|
||||||
|
k_fatal_halt(K_ERR_KERNEL_PANIC);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (zsock_listen(sock, 2) < 0) {
|
||||||
|
LOG_ERR("error: listen: %d", errno);
|
||||||
|
k_fatal_halt(K_ERR_KERNEL_PANIC);
|
||||||
|
}
|
||||||
|
|
||||||
|
LOG_INF("Upsilon waiting on %d", port);
|
||||||
|
|
||||||
|
return sock;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
server_accept_client(int server)
|
||||||
|
{
|
||||||
|
int client;
|
||||||
|
struct sockaddr_in addr;
|
||||||
|
socklen_t len = sizeof(addr);
|
||||||
|
|
||||||
|
do {
|
||||||
|
client = zsock_accept(server, (struct sockaddr *)&addr, &len);
|
||||||
|
if (client < 0)
|
||||||
|
LOG_WRN("error in accept: %d", errno);
|
||||||
|
} while (client < 0);
|
||||||
|
|
||||||
|
char ipaddr[32];
|
||||||
|
zsock_inet_ntop(addr.sin_family, &addr.sin_addr, ipaddr, sizeof(ipaddr));
|
||||||
|
LOG_INF("Connection received from %s", ipaddr);
|
||||||
|
return client;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool
|
||||||
|
client_read_into_buf(int sock, struct clireadbuf *buf)
|
||||||
|
{
|
||||||
|
if (buf->st == MSG_READY) {
|
||||||
|
LOG_WRN("%s called while MSG_READY: misuse", __func__);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!buf_read_sock(sock, &buf->b))
|
||||||
|
return false;
|
||||||
|
|
||||||
|
if (buf->b.left == 0) switch (buf->st) {
|
||||||
|
case WAIT_ON_HEADER: {
|
||||||
|
uint16_t len;
|
||||||
|
memcpy(&len, buf->buf, sizeof(len));
|
||||||
|
buf->b.left = ntohs(len);
|
||||||
|
buf->st = READING_CLIENT;
|
||||||
|
break;
|
||||||
|
} case READING_CLIENT:
|
||||||
|
buf->st = MSG_READY;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
client_buf_reset(struct clireadbuf *buf)
|
||||||
|
{
|
||||||
|
buf->st = WAIT_ON_HEADER;
|
||||||
|
buf->b.p = buf->buf;
|
||||||
|
buf->b.left = sizeof(uint16_t);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,19 @@
|
||||||
|
#pragma once
|
||||||
|
#include "buf.h"
|
||||||
|
int server_init_sock(int port);
|
||||||
|
int server_accept_client(int server);
|
||||||
|
|
||||||
|
#define CLIREADBUF_SIZ 1024
|
||||||
|
enum clireadbuf_state {
|
||||||
|
WAIT_ON_HEADER,
|
||||||
|
READING_CLIENT,
|
||||||
|
MSG_READY
|
||||||
|
};
|
||||||
|
struct clireadbuf {
|
||||||
|
struct bufptr b;
|
||||||
|
enum clireadbuf_state st;
|
||||||
|
char buf[CLIREADBUF_SIZ];
|
||||||
|
};
|
||||||
|
|
||||||
|
bool client_read_into_buf(int sock, struct clireadbuf *buf);
|
||||||
|
void client_buf_reset(struct clireadbuf *buf);
|
||||||
Loading…
Reference in New Issue