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
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* 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/.
|
||||
/* SPI master.
|
||||
* Written by Peter McGoron, 2022.
|
||||
*/
|
||||
|
||||
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
|
||||
*
|
||||
* Copyright (c) 2010 Serge Zaitsev
|
||||
|
|
|
|||
|
|
@ -1,26 +1,58 @@
|
|||
#include <zephyr/zephyr.h>
|
||||
#include <errno.h>
|
||||
#include <zephyr/net/socket.h>
|
||||
#include <zephyr/kernel.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
|
||||
main(void)
|
||||
{
|
||||
uint32_t v = 0;
|
||||
uint32_t r = 0;
|
||||
LOG_PRINTK("hello, world\n");
|
||||
int srv = server_init_sock(PORT);
|
||||
|
||||
for (;;) {
|
||||
k_sleep(K_MSEC(1000));
|
||||
#if 0 // ADC code
|
||||
*adc_conv[0] = v;
|
||||
v = !v;
|
||||
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);
|
||||
int cli = server_get_client(server_sock);
|
||||
process_client(cli);
|
||||
LOG_INF("Closing client socket");
|
||||
zsock_close(cli);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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