// Gabor_filter_lh.h
// 2016/07/24
// 2016/07/25 : 右白線検出用のGabor Filterの重みを追加
// 2016/07/27 : 右白線検出用配列と左白線検出用配列を統合
//
#ifndef __Gabor_filter_lh_H__
#define __Gabor_filter_lh_H__
//#define HORIZONTAL_PIXEL_WIDTH 800
//#define VERTICAL_PIXEL_WIDTH 600
#define HORIZONTAL_PIXEL_WIDTH 640
#define VERTICAL_PIXEL_WIDTH 480
//#define HORIZONTAL_PIXEL_WIDTH 64
//#define VERTICAL_PIXEL_WIDTH 48
#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)
#define ARRAY_SIZE 9
#define RIGHT_WEIGHT 1
#define LEFT_WEIGHT 0
const int gabor_weight[2][ARRAY_SIZE][ARRAY_SIZE] = { // 左白線検出用+右白線検出用
{
{0,-2,-9,-17,-19,-13,-4,0,1},
{-5,-15,-28,-30,-9,16,22,14,5},
{-8,-12,5,57,112,115,69,24,4},
{5,35,108,186,190,105,17,-16,-12},
{20,59,101,82,-18,-109,-113,-61,-19},
{10,9,-33,-125,-203,-186,-100,-29,-2},
{-5,-27,-71,-111,-100,-43,5,16,9},
{-5,-13,-19,-9,18,36,30,15,4},
{-1,0,5,14,20,17,8,2,0}
},
{
{0,3,11,10,-4,-15,-10,-2,1},
{-4,-1,23,53,37,-11,-29,-14,-2},
{-11,-26,-5,82,136,70,-15,-29,-10},
{-8,-51,-93,-13,154,186,70,-11,-15},
{7,-30,-133,-174,-18,154,136,37,-4},
{16,18,-57,-183,-174,-13,82,53,10},
{10,31,24,-57,-133,-93,-5,23,11},
{1,13,31,18,-30,-51,-26,-1,3},
{-1,1,10,16,7,-8,-11,-4,0}
}
};
const float gabor_fweight[2][ARRAY_SIZE][ARRAY_SIZE] = { // 左白線検出用+右白線検出用(float)
{
{-0.000387,-0.009416,-0.034769,-0.067138,-0.075984,-0.04953,-0.015067,0.001684,0.003258},
{-0.017724,-0.057864,-0.111235,-0.116438,-0.035878,0.061351,0.087704,0.054213,0.019079},
{-0.031458,-0.048283,0.018458,0.22454,0.438323,0.448468,0.269024,0.09288,0.014896},
{0.018804,0.137564,0.420198,0.727096,0.740714,0.410208,0.065614,-0.0622,-0.046793},
{0.078413,0.231233,0.393865,0.318885,-0.069756,-0.42599,-0.439616,-0.23833,-0.07577},
{0.039486,0.034327,-0.12741,-0.490047,-0.792569,-0.726876,-0.392411,-0.114968,-0.009538},
{-0.020254,-0.10421,-0.278203,-0.434837,-0.39232,-0.167537,0.020893,0.064293,0.035078},
{-0.019134,-0.050825,-0.073897,-0.03345,0.068666,0.138881,0.118933,0.057811,0.016539},
{-0.002471,0.000988,0.020414,0.055516,0.078527,0.065056,0.031089,0.007002,-0.000496}
},
{
{-0.000442,0.013535,0.042156,0.041006,-0.016289,-0.059269,-0.039775,-0.006967,0.002935},
{-0.01529,-0.004044,0.090353,0.205079,0.145379,-0.042085,-0.111597,-0.054579,-0.006967},
{-0.041345,-0.102069,-0.019676,0.320331,0.529862,0.273733,-0.057749,-0.111597,-0.039775},
{-0.033072,-0.201134,-0.361867,-0.050836,0.603151,0.72707,0.273733,-0.042085,-0.059269},
{0.026968,-0.117249,-0.519669,-0.68136,-0.069756,0.603151,0.529862,0.145379,-0.016289},
{0.063308,0.069678,-0.220769,-0.713083,-0.68136,-0.050836,0.320331,0.205079,0.041006},
{0.037404,0.119202,0.095611,-0.220769,-0.519669,-0.361867,-0.019676,0.090353,0.042156},
{0.004415,0.051325,0.119202,0.069678,-0.117249,-0.201134,-0.102069,-0.004044,0.013535},
{-0.003687,0.004415,0.037404,0.063308,0.026968,-0.033072,-0.041345,-0.01529,-0.000442}
}
};
#endif
// Gabor_fiter_lh.cpp
// 2016/07/23 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
// 2016/07/27 : 右白線検出用配列と左白線検出用配列を統合
//
#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>
#include "Gabor_filter_lh.h"
int conv_rgb2y(int rgb);
int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins,
hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL){
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> gabor;
hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;
int gray_pix, val, i, j, x, y;
do { // user が 1になった時にフレームがスタートする
ins >> pix;
} while(pix.user == 0);
for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
if (!(x==0 && y==0)) // 最初の入力はすでに入力されている
ins >> pix; // AXI4-Stream からの入力
mbuf.shift_left(); // mbuf の列を1ビット左シフト
for(i=ARRAY_SIZE-2; i>=0; --i){
mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
}
gray_pix = conv_rgb2y(pix.data);
mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);
// LineBuffer の更新
linebuf.shift_down(x);
linebuf.insert_bottom(gray_pix, x);
// Gabor filter の演算
for (j=0, val=0; j<ARRAY_SIZE-1; j++){
for (i=0; i<ARRAY_SIZE-1; i++){
val += gabor_weight[(int)RorL][j][i] * mbuf(ARRAY_SIZE-1-j,i);
}
}
val = val/256; // 256倍してあるので、1/256して戻す
if (val<0)
//val = -val; // 絶対値
val = 0; // マイナスの値を0に丸める
else if (val>255)
val = 255;
// Gabor filter・データの書き込み
gabor.data = (val<<16)+(val<<8)+val;
// 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
gabor.data = 0;
if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
gabor.user = 1;
else
gabor.user = 0;
if (x == (HORIZONTAL_PIXEL_WIDTH-1)) // 行の最後で TLAST をアサートする
gabor.last = 1;
else
gabor.last = 0;
outs << gabor; // AXI4-Stream へ出力
}
}
return(0);
}
// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y = 0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
// 2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y(int rgb){
int r, g, b, y_f;
int y;
b = rgb & 0xff;
g = (rgb>>8) & 0xff;
r = (rgb>>16) & 0xff;
y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
y = y_f >> 8; // 256で割る
return(y);
}
// Gabor_filter_lh_tb.cpp
// 2016/07/24 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>
#include "Gabor_filter_lh.h"
#include "bmp_header.h"
int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL);
int conv_rgb2y_soft(int rgb);
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL);
#define CLOCK_PERIOD 10
#define RIGHT_OR_LEFT LEFT_WEIGHT
#define BMP_FILE_NAME "road_4.bmp"
int main()
{
using namespace std;
hls::stream<ap_axis<32,1,1,1> > ins;
hls::stream<ap_axis<32,1,1,1> > ins_soft;
hls::stream<ap_axis<32,1,1,1> > outs;
hls::stream<ap_axis<32,1,1,1> > outs_soft;
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> vals;
ap_axis<32,1,1,1> vals_soft;
int m_seq = 1; // M系列の値
int i;
int xor_shift;
BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
FILE *fbmpr, *fbmpw, *fbmpwf;
int *rd_bmp, *hw_gabor, *sw_gabor;
int blue, green, red;
ap_uint<1> r_l;
if ((fbmpr = fopen(BMP_FILE_NAME, "rb")) == NULL){ // test.bmp をオープン
fprintf(stderr, "Can't open test.bmp by binary read mode\n");
exit(1);
}
// bmpヘッダの読み出し
fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);
// ピクセルを入れるメモリをアロケートする
if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate rd_bmp memory\n");
exit(1);
}
if ((hw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate hw_gabor memory\n");
exit(1);
}
if ((sw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate hw_gabor memory\n");
exit(1);
}
// rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = fgetc(fbmpr);
green = fgetc(fbmpr);
red = fgetc(fbmpr);
rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
}
}
fclose(fbmpr);
// ins に入力データを用意する
for(int i=0; i<5; i++){ // dummy data
pix.user = 0;
pix.data = i;
ins << pix;
}
for(int j=0; j < bmpihr.biHeight; j++){
for(i=0; i < bmpihr.biWidth; i++){
pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];
if (j==0 && i==0) // 最初のデータの時に TUSER を 1 にする
pix.user = 1;
else
pix.user = 0;
if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
pix.last = 1;
else
pix.last = 0;
ins << pix;
ins_soft << pix;
}
}
r_l = (ap_uint<1>)RIGHT_OR_LEFT;
Gabor_filter_lh(ins, outs, r_l);
Gabor_filter_lh_soft(ins_soft, outs_soft, r_l);
// ハードウェアとソフトウェアのラプラシアン・フィルタの値のチェック
cout << endl;
cout << "outs" << endl;
for(int j=0; j < bmpihr.biHeight; j++){
for(i=0; i < bmpihr.biWidth; i++){
outs >> vals;
outs_soft >> vals_soft;
ap_int<32> val = vals.data;
ap_int<32> val_soft = vals_soft.data;
hw_gabor[(j*bmpihr.biWidth)+i] = (int)val;
sw_gabor[(j*bmpihr.biWidth)+i] = (int)val_soft;
if ((double)pow((double)(val&0xff)-(val_soft&0xff),(double)2) > 4){ // 2乗誤差が4よりも大きい
printf("ERROR HW and SW results mismatch i = %ld, j = %ld, HW = %08x, SW = %08x\n", i, j, (int)val, (int)val_soft);
//return(1);
}
//if (vals.last)
//cout << "AXI-Stream is end" << endl;
}
}
cout << "Success HW and SW results match" << endl;
cout << endl;
// ハードウェアのラプラシアンフィルタの結果を temp_gabor.bmp へ出力する
if ((fbmpw=fopen("temp_gabor.bmp", "wb")) == NULL){
fprintf(stderr, "Can't open temp_gabor.bmp by binary write mode\n");
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
// RGB データの書き込み、逆順にする
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpw);
fputc(green, fbmpw);
fputc(red, fbmpw);
}
}
fclose(fbmpw);
// ソフトウェアのラプラシアンフィルタの結果を temp_gabor_float.bmp へ出力する
if ((fbmpwf=fopen("temp_gabor_float.bmp", "wb")) == NULL){
fprintf(stderr, "Can't open temp_gabor_float.bmp by binary write mode\n");
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpwf);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpwf);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpwf);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpwf);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpwf);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpwf);
// RGB データの書き込み、逆順にする
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpwf);
fputc(green, fbmpwf);
fputc(red, fbmpwf);
}
}
fclose(fbmpwf);
free(rd_bmp);
free(hw_gabor);
return 0;
}
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL){
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> gabor;
hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;
int gray_pix, val, i, j, x, y;
float valf;
do { // user が 1になった時にフレームがスタートする
ins >> pix;
} while(pix.user == 0);
for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
if (!(x==0 && y==0)) // 最初の入力はすでに入力されている
ins >> pix; // AXI4-Stream からの入力
mbuf.shift_left(); // mbuf の列を1ビット左シフト
for(i=ARRAY_SIZE-2; i>=0; --i){
mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
}
gray_pix = conv_rgb2y_soft(pix.data);
mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);
// LineBuffer の更新
linebuf.shift_down(x);
linebuf.insert_bottom(gray_pix, x);
// Gabor filter の演算
for (j=0, valf=0; j<ARRAY_SIZE-1; j++){
for (i=0; i<ARRAY_SIZE-1; i++){
valf += gabor_fweight[(int)RorL][j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
}
}
val = (int)valf;
if (val<0)
//val = -val; // 絶対値
val = 0; // マイナスの値を0に丸める
else if (val>255)
val = 255;
// Gabor filter・データの書き込み
gabor.data = (val<<16)+(val<<8)+val;
// 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
gabor.data = 0;
if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
gabor.user = 1;
else
gabor.user = 0;
if (x == (HORIZONTAL_PIXEL_WIDTH-1)) // 行の最後で TLAST をアサートする
gabor.last = 1;
else
gabor.last = 0;
outs << gabor; // AXI4-Stream へ出力
}
}
return(0);
}
// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y = 0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
// 2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y_soft(int rgb){
int r, g, b, y_f;
int y;
b = rgb & 0xff;
g = (rgb>>8) & 0xff;
r = (rgb>>16) & 0xff;
y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
y = y_f >> 8; // 256で割る
return(y);
// Gabor_filter_lh.h
// 2016/07/24
// 2016/07/25 : 右白線検出用のGabor Filterの重みを追加
//
#ifndef __Gabor_filter_lh_H__
#define __Gabor_filter_lh_H__
//#define HORIZONTAL_PIXEL_WIDTH 800
//#define VERTICAL_PIXEL_WIDTH 600
#define HORIZONTAL_PIXEL_WIDTH 640
#define VERTICAL_PIXEL_WIDTH 480
//#define HORIZONTAL_PIXEL_WIDTH 64
//#define VERTICAL_PIXEL_WIDTH 48
#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)
#define ARRAY_SIZE 9
#define RIGHT_WEIGHT 1
#define LEFT_WEIGHT 0
const int gabor_weight_left[ARRAY_SIZE][ARRAY_SIZE] = { // 左側の白線検出用
{0,-2,-9,-17,-19,-13,-4,0,1},
{-5,-15,-28,-30,-9,16,22,14,5},
{-8,-12,5,57,112,115,69,24,4},
{5,35,108,186,190,105,17,-16,-12},
{20,59,101,82,-18,-109,-113,-61,-19},
{10,9,-33,-125,-203,-186,-100,-29,-2},
{-5,-27,-71,-111,-100,-43,5,16,9},
{-5,-13,-19,-9,18,36,30,15,4},
{-1,0,5,14,20,17,8,2,0}
};
const float gabor_fweight_left[ARRAY_SIZE][ARRAY_SIZE] = { // 左側の白線検出用(float)
{-0.000387,-0.009416,-0.034769,-0.067138,-0.075984,-0.04953,-0.015067,0.001684,0.003258},
{-0.017724,-0.057864,-0.111235,-0.116438,-0.035878,0.061351,0.087704,0.054213,0.019079},
{-0.031458,-0.048283,0.018458,0.22454,0.438323,0.448468,0.269024,0.09288,0.014896},
{0.018804,0.137564,0.420198,0.727096,0.740714,0.410208,0.065614,-0.0622,-0.046793},
{0.078413,0.231233,0.393865,0.318885,-0.069756,-0.42599,-0.439616,-0.23833,-0.07577},
{0.039486,0.034327,-0.12741,-0.490047,-0.792569,-0.726876,-0.392411,-0.114968,-0.009538},
{-0.020254,-0.10421,-0.278203,-0.434837,-0.39232,-0.167537,0.020893,0.064293,0.035078},
{-0.019134,-0.050825,-0.073897,-0.03345,0.068666,0.138881,0.118933,0.057811,0.016539},
{-0.002471,0.000988,0.020414,0.055516,0.078527,0.065056,0.031089,0.007002,-0.000496}
};
const int gabor_weight_right[ARRAY_SIZE][ARRAY_SIZE] = { // 右側の白線検出用
{0,3,11,10,-4,-15,-10,-2,1},
{-4,-1,23,53,37,-11,-29,-14,-2},
{-11,-26,-5,82,136,70,-15,-29,-10},
{-8,-51,-93,-13,154,186,70,-11,-15},
{7,-30,-133,-174,-18,154,136,37,-4},
{16,18,-57,-183,-174,-13,82,53,10},
{10,31,24,-57,-133,-93,-5,23,11},
{1,13,31,18,-30,-51,-26,-1,3},
{-1,1,10,16,7,-8,-11,-4,0}
};
const float gabor_fweight_right[ARRAY_SIZE][ARRAY_SIZE] = { // 右側の白線検出用(float)
{-0.000442,0.013535,0.042156,0.041006,-0.016289,-0.059269,-0.039775,-0.006967,0.002935},
{-0.01529,-0.004044,0.090353,0.205079,0.145379,-0.042085,-0.111597,-0.054579,-0.006967},
{-0.041345,-0.102069,-0.019676,0.320331,0.529862,0.273733,-0.057749,-0.111597,-0.039775},
{-0.033072,-0.201134,-0.361867,-0.050836,0.603151,0.72707,0.273733,-0.042085,-0.059269},
{0.026968,-0.117249,-0.519669,-0.68136,-0.069756,0.603151,0.529862,0.145379,-0.016289},
{0.063308,0.069678,-0.220769,-0.713083,-0.68136,-0.050836,0.320331,0.205079,0.041006},
{0.037404,0.119202,0.095611,-0.220769,-0.519669,-0.361867,-0.019676,0.090353,0.042156},
{0.004415,0.051325,0.119202,0.069678,-0.117249,-0.201134,-0.102069,-0.004044,0.013535},
{-0.003687,0.004415,0.037404,0.063308,0.026968,-0.033072,-0.041345,-0.01529,-0.000442}
};
#endif
// Gabor_fiter_lh.cpp
// 2016/07/23 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
//
#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>
#include "Gabor_filter_lh.h"
int conv_rgb2y(int rgb);
int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins,
hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL){
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> gabor;
hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;
int gray_pix, val, i, j, x, y;
do { // user が 1になった時にフレームがスタートする
ins >> pix;
} while(pix.user == 0);
for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
if (!(x==0 && y==0)) // 最初の入力はすでに入力されている
ins >> pix; // AXI4-Stream からの入力
mbuf.shift_left(); // mbuf の列を1ビット左シフト
for(i=ARRAY_SIZE-2; i>=0; --i){
mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
}
gray_pix = conv_rgb2y(pix.data);
mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);
// LineBuffer の更新
linebuf.shift_down(x);
linebuf.insert_bottom(gray_pix, x);
// Gabor filter の演算
for (j=0, val=0; j<ARRAY_SIZE-1; j++){
for (i=0; i<ARRAY_SIZE-1; i++){
if (RorL == LEFT_WEIGHT) // 左白線検出用重みと右白線検出用重みの選択
val += gabor_weight_left[j][i] * mbuf(ARRAY_SIZE-1-j,i);
else
val += gabor_weight_right[j][i] * mbuf(ARRAY_SIZE-1-j,i);
}
}
val = val/256; // 256倍してあるので、1/256して戻す
if (val<0)
//val = -val; // 絶対値
val = 0; // マイナスの値を0に丸める
else if (val>255)
val = 255;
// Gabor filter・データの書き込み
gabor.data = (val<<16)+(val<<8)+val;
// 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
gabor.data = 0;
if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
gabor.user = 1;
else
gabor.user = 0;
if (x == (HORIZONTAL_PIXEL_WIDTH-1)) // 行の最後で TLAST をアサートする
gabor.last = 1;
else
gabor.last = 0;
outs << gabor; // AXI4-Stream へ出力
}
}
return(0);
}
// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y = 0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
// 2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y(int rgb){
int r, g, b, y_f;
int y;
b = rgb & 0xff;
g = (rgb>>8) & 0xff;
r = (rgb>>16) & 0xff;
y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
y = y_f >> 8; // 256で割る
return(y);
}
// Gabor_filter_lh_tb.cpp
// 2016/07/24 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>
#include "Gabor_filter_lh.h"
#include "bmp_header.h"
int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL);
int conv_rgb2y_soft(int rgb);
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL);
#define CLOCK_PERIOD 10
#define RIGHT_OR_LEFT RIGHT_WEIGHT
int main()
{
using namespace std;
hls::stream<ap_axis<32,1,1,1> > ins;
hls::stream<ap_axis<32,1,1,1> > ins_soft;
hls::stream<ap_axis<32,1,1,1> > outs;
hls::stream<ap_axis<32,1,1,1> > outs_soft;
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> vals;
ap_axis<32,1,1,1> vals_soft;
int m_seq = 1; // M系列の値
int i;
int xor_shift;
BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
FILE *fbmpr, *fbmpw, *fbmpwf;
int *rd_bmp, *hw_gabor, *sw_gabor;
int blue, green, red;
ap_uint<1> r_l;
if ((fbmpr = fopen("road_1.bmp", "rb")) == NULL){ // test.bmp をオープン
fprintf(stderr, "Can't open test.bmp by binary read mode\n");
exit(1);
}
// bmpヘッダの読み出し
fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);
// ピクセルを入れるメモリをアロケートする
if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate rd_bmp memory\n");
exit(1);
}
if ((hw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate hw_gabor memory\n");
exit(1);
}
if ((sw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate hw_gabor memory\n");
exit(1);
}
// rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = fgetc(fbmpr);
green = fgetc(fbmpr);
red = fgetc(fbmpr);
rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
}
}
fclose(fbmpr);
// ins に入力データを用意する
for(int i=0; i<5; i++){ // dummy data
pix.user = 0;
pix.data = i;
ins << pix;
}
for(int j=0; j < bmpihr.biHeight; j++){
for(i=0; i < bmpihr.biWidth; i++){
pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];
if (j==0 && i==0) // 最初のデータの時に TUSER を 1 にする
pix.user = 1;
else
pix.user = 0;
if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
pix.last = 1;
else
pix.last = 0;
ins << pix;
ins_soft << pix;
}
}
r_l = (ap_uint<1>)RIGHT_OR_LEFT;
Gabor_filter_lh(ins, outs, r_l);
Gabor_filter_lh_soft(ins_soft, outs_soft, r_l);
// ハードウェアとソフトウェアのラプラシアン・フィルタの値のチェック
cout << endl;
cout << "outs" << endl;
for(int j=0; j < bmpihr.biHeight; j++){
for(i=0; i < bmpihr.biWidth; i++){
outs >> vals;
outs_soft >> vals_soft;
ap_int<32> val = vals.data;
ap_int<32> val_soft = vals_soft.data;
hw_gabor[(j*bmpihr.biWidth)+i] = (int)val;
sw_gabor[(j*bmpihr.biWidth)+i] = (int)val_soft;
if ((double)pow((double)(val&0xff)-(val_soft&0xff),(double)2) > 4){ // 2乗誤差が4よりも大きい
printf("ERROR HW and SW results mismatch i = %ld, j = %ld, HW = %08x, SW = %08x\n", i, j, (int)val, (int)val_soft);
//return(1);
}
//if (vals.last)
//cout << "AXI-Stream is end" << endl;
}
}
cout << "Success HW and SW results match" << endl;
cout << endl;
// ハードウェアのラプラシアンフィルタの結果を temp_gabor.bmp へ出力する
if ((fbmpw=fopen("temp_gabor.bmp", "wb")) == NULL){
fprintf(stderr, "Can't open temp_gabor.bmp by binary write mode\n");
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
// RGB データの書き込み、逆順にする
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpw);
fputc(green, fbmpw);
fputc(red, fbmpw);
}
}
fclose(fbmpw);
// ソフトウェアのラプラシアンフィルタの結果を temp_gabor_float.bmp へ出力する
if ((fbmpwf=fopen("temp_gabor_float.bmp", "wb")) == NULL){
fprintf(stderr, "Can't open temp_gabor_float.bmp by binary write mode\n");
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpwf);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpwf);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpwf);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpwf);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpwf);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpwf);
// RGB データの書き込み、逆順にする
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpwf);
fputc(green, fbmpwf);
fputc(red, fbmpwf);
}
}
fclose(fbmpwf);
free(rd_bmp);
free(hw_gabor);
return 0;
}
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL){
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> gabor;
hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;
int gray_pix, val, i, j, x, y;
float valf;
do { // user が 1になった時にフレームがスタートする
ins >> pix;
} while(pix.user == 0);
for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
if (!(x==0 && y==0)) // 最初の入力はすでに入力されている
ins >> pix; // AXI4-Stream からの入力
mbuf.shift_left(); // mbuf の列を1ビット左シフト
for(i=ARRAY_SIZE-2; i>=0; --i){
mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
}
gray_pix = conv_rgb2y_soft(pix.data);
mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);
// LineBuffer の更新
linebuf.shift_down(x);
linebuf.insert_bottom(gray_pix, x);
// Gabor filter の演算
for (j=0, valf=0; j<ARRAY_SIZE-1; j++){
for (i=0; i<ARRAY_SIZE-1; i++){
if (RorL == LEFT_WEIGHT) // 左白線検出用重みと右白線検出用重みの選択
valf += gabor_fweight_left[j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
else
valf += gabor_fweight_right[j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
}
}
val = (int)valf;
if (val<0)
//val = -val; // 絶対値
val = 0; // マイナスの値を0に丸める
else if (val>255)
val = 255;
// Gabor filter・データの書き込み
gabor.data = (val<<16)+(val<<8)+val;
// 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
gabor.data = 0;
if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
gabor.user = 1;
else
gabor.user = 0;
if (x == (HORIZONTAL_PIXEL_WIDTH-1)) // 行の最後で TLAST をアサートする
gabor.last = 1;
else
gabor.last = 0;
outs << gabor; // AXI4-Stream へ出力
}
}
return(0);
}
// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y = 0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
// 2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y_soft(int rgb){
int r, g, b, y_f;
int y;
b = rgb & 0xff;
g = (rgb>>8) & 0xff;
r = (rgb>>16) & 0xff;
y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
y = y_f >> 8; // 256で割る
return(y);
}
// Gabor_filter_lh.h
// 2016/07/24
#ifndef __Gabor_filter_lh_H__
#define __Gabor_filter_lh_H__
//#define HORIZONTAL_PIXEL_WIDTH 800
//#define VERTICAL_PIXEL_WIDTH 600
#define HORIZONTAL_PIXEL_WIDTH 640
#define VERTICAL_PIXEL_WIDTH 480
//#define HORIZONTAL_PIXEL_WIDTH 64
//#define VERTICAL_PIXEL_WIDTH 48
#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)
#define ARRAY_SIZE 9
const int gabor_weight[ARRAY_SIZE][ARRAY_SIZE] = { // 左側の白線検出用
{0,-2,-9,-17,-19,-13,-4,0,1},
{-5,-15,-28,-30,-9,16,22,14,5},
{-8,-12,5,57,112,115,69,24,4},
{5,35,108,186,190,105,17,-16,-12},
{20,59,101,82,-18,-109,-113,-61,-19},
{10,9,-33,-125,-203,-186,-100,-29,-2},
{-5,-27,-71,-111,-100,-43,5,16,9},
{-5,-13,-19,-9,18,36,30,15,4},
{-1,0,5,14,20,17,8,2,0}
};
const float gabor_fweight[ARRAY_SIZE][ARRAY_SIZE] = { // 左側の白線検出用(float)
{-0.000387,-0.009416,-0.034769,-0.067138,-0.075984,-0.04953,-0.015067,0.001684,0.003258},
{-0.017724,-0.057864,-0.111235,-0.116438,-0.035878,0.061351,0.087704,0.054213,0.019079},
{-0.031458,-0.048283,0.018458,0.22454,0.438323,0.448468,0.269024,0.09288,0.014896},
{0.018804,0.137564,0.420198,0.727096,0.740714,0.410208,0.065614,-0.0622,-0.046793},
{0.078413,0.231233,0.393865,0.318885,-0.069756,-0.42599,-0.439616,-0.23833,-0.07577},
{0.039486,0.034327,-0.12741,-0.490047,-0.792569,-0.726876,-0.392411,-0.114968,-0.009538},
{-0.020254,-0.10421,-0.278203,-0.434837,-0.39232,-0.167537,0.020893,0.064293,0.035078},
{-0.019134,-0.050825,-0.073897,-0.03345,0.068666,0.138881,0.118933,0.057811,0.016539},
{-0.002471,0.000988,0.020414,0.055516,0.078527,0.065056,0.031089,0.007002,-0.000496}
};
#endif
// Gabor_fiter_lh.cpp
// 2016/07/23 by marsee
//
#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>
#include "Gabor_filter_lh.h"
int conv_rgb2y(int rgb);
int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins,
hls::stream<ap_axis<32,1,1,1> >& outs){
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> gabor;
hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;
int gray_pix, val, i, j, x, y;
do { // user が 1になった時にフレームがスタートする
ins >> pix;
} while(pix.user == 0);
for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
if (!(x==0 && y==0)) // 最初の入力はすでに入力されている
ins >> pix; // AXI4-Stream からの入力
mbuf.shift_left(); // mbuf の列を1ビット左シフト
for(i=ARRAY_SIZE-2; i>=0; --i){
mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
}
gray_pix = conv_rgb2y(pix.data);
mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);
// LineBuffer の更新
linebuf.shift_down(x);
linebuf.insert_bottom(gray_pix, x);
// Gabor filter の演算
for (j=0, val=0; j<ARRAY_SIZE-1; j++){
for (i=0; i<ARRAY_SIZE-1; i++){
val += gabor_weight[j][i] * mbuf(ARRAY_SIZE-1-j,i);
}
}
val = val/256; // 256倍してあるので、1/256して戻す
if (val<0)
//val = -val; // 絶対値
val = 0; // マイナスの値を0に丸める
else if (val>255)
val = 255;
// Gabor filter・データの書き込み
gabor.data = (val<<16)+(val<<8)+val;
// 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
gabor.data = 0;
if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
gabor.user = 1;
else
gabor.user = 0;
if (x == (HORIZONTAL_PIXEL_WIDTH-1)) // 行の最後で TLAST をアサートする
gabor.last = 1;
else
gabor.last = 0;
outs << gabor; // AXI4-Stream へ出力
}
}
return(0);
}
// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y = 0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
// 2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y(int rgb){
int r, g, b, y_f;
int y;
b = rgb & 0xff;
g = (rgb>>8) & 0xff;
r = (rgb>>16) & 0xff;
y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
y = y_f >> 8; // 256で割る
return(y);
}
// Gabor_filter_lh_tb.cpp
// 2016/07/24 by marsee
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>
#include "Gabor_filter_lh.h"
#include "bmp_header.h"
int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs);
int conv_rgb2y_soft(int rgb);
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs);
#define CLOCK_PERIOD 10
int main()
{
using namespace std;
hls::stream<ap_axis<32,1,1,1> > ins;
hls::stream<ap_axis<32,1,1,1> > ins_soft;
hls::stream<ap_axis<32,1,1,1> > outs;
hls::stream<ap_axis<32,1,1,1> > outs_soft;
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> vals;
ap_axis<32,1,1,1> vals_soft;
int m_seq = 1; // M系列の値
int i;
int xor_shift;
BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
FILE *fbmpr, *fbmpw, *fbmpwf;
int *rd_bmp, *hw_gabor, *sw_gabor;
int blue, green, red;
if ((fbmpr = fopen("road_1.bmp", "rb")) == NULL){ // test.bmp をオープン
fprintf(stderr, "Can't open test.bmp by binary read mode\n");
exit(1);
}
// bmpヘッダの読み出し
fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);
// ピクセルを入れるメモリをアロケートする
if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate rd_bmp memory\n");
exit(1);
}
if ((hw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate hw_gabor memory\n");
exit(1);
}
if ((sw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate hw_gabor memory\n");
exit(1);
}
// rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = fgetc(fbmpr);
green = fgetc(fbmpr);
red = fgetc(fbmpr);
rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
}
}
fclose(fbmpr);
// ins に入力データを用意する
for(int i=0; i<5; i++){ // dummy data
pix.user = 0;
pix.data = i;
ins << pix;
}
for(int j=0; j < bmpihr.biHeight; j++){
for(i=0; i < bmpihr.biWidth; i++){
pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];
if (j==0 && i==0) // 最初のデータの時に TUSER を 1 にする
pix.user = 1;
else
pix.user = 0;
if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
pix.last = 1;
else
pix.last = 0;
ins << pix;
ins_soft << pix;
}
}
Gabor_filter_lh(ins, outs);
Gabor_filter_lh_soft(ins_soft, outs_soft);
// ハードウェアとソフトウェアのラプラシアン・フィルタの値のチェック
cout << endl;
cout << "outs" << endl;
for(int j=0; j < bmpihr.biHeight; j++){
for(i=0; i < bmpihr.biWidth; i++){
outs >> vals;
outs_soft >> vals_soft;
ap_int<32> val = vals.data;
ap_int<32> val_soft = vals_soft.data;
hw_gabor[(j*bmpihr.biWidth)+i] = (int)val;
sw_gabor[(j*bmpihr.biWidth)+i] = (int)val_soft;
if ((double)pow((double)(val&0xff)-(val_soft&0xff),(double)2) > 4){ // 2乗誤差が4よりも大きい
printf("ERROR HW and SW results mismatch i = %ld, j = %ld, HW = %08x, SW = %08x\n", i, j, (int)val, (int)val_soft);
//return(1);
}
//if (vals.last)
//cout << "AXI-Stream is end" << endl;
}
}
cout << "Success HW and SW results match" << endl;
cout << endl;
// ハードウェアのラプラシアンフィルタの結果を temp_gabor.bmp へ出力する
if ((fbmpw=fopen("temp_gabor.bmp", "wb")) == NULL){
fprintf(stderr, "Can't open temp_gabor.bmp by binary write mode\n");
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
// RGB データの書き込み、逆順にする
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpw);
fputc(green, fbmpw);
fputc(red, fbmpw);
}
}
fclose(fbmpw);
// ソフトウェアのラプラシアンフィルタの結果を temp_gabor_float.bmp へ出力する
if ((fbmpwf=fopen("temp_gabor_float.bmp", "wb")) == NULL){
fprintf(stderr, "Can't open temp_gabor_float.bmp by binary write mode\n");
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpwf);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpwf);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpwf);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpwf);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpwf);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpwf);
// RGB データの書き込み、逆順にする
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpwf);
fputc(green, fbmpwf);
fputc(red, fbmpwf);
}
}
fclose(fbmpwf);
free(rd_bmp);
free(hw_gabor);
return 0;
}
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs){
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> gabor;
hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;
int gray_pix, val, i, j, x, y;
float valf;
do { // user が 1になった時にフレームがスタートする
ins >> pix;
} while(pix.user == 0);
for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
if (!(x==0 && y==0)) // 最初の入力はすでに入力されている
ins >> pix; // AXI4-Stream からの入力
mbuf.shift_left(); // mbuf の列を1ビット左シフト
for(i=ARRAY_SIZE-2; i>=0; --i){
mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
}
gray_pix = conv_rgb2y_soft(pix.data);
mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);
// LineBuffer の更新
linebuf.shift_down(x);
linebuf.insert_bottom(gray_pix, x);
// Gabor filter の演算
for (j=0, valf=0; j<ARRAY_SIZE-1; j++){
for (i=0; i<ARRAY_SIZE-1; i++){
valf += gabor_fweight[j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
}
}
val = (int)valf;
if (val<0)
//val = -val; // 絶対値
val = 0; // マイナスの値を0に丸める
else if (val>255)
val = 255;
// Gabor filter・データの書き込み
gabor.data = (val<<16)+(val<<8)+val;
// 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
gabor.data = 0;
if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
gabor.user = 1;
else
gabor.user = 0;
if (x == (HORIZONTAL_PIXEL_WIDTH-1)) // 行の最後で TLAST をアサートする
gabor.last = 1;
else
gabor.last = 0;
outs << gabor; // AXI4-Stream へ出力
}
}
return(0);
}
// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y = 0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
// 2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y_soft(int rgb){
int r, g, b, y_f;
int y;
b = rgb & 0xff;
g = (rgb>>8) & 0xff;
r = (rgb>>16) & 0xff;
y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
y = y_f >> 8; // 256で割る
return(y);
}
左モーター
(測定データ)10.8 sec / 20 回転、308 Hz , 298 Hz (平均 303 Hz)
20 回転 / 10.8 sec ≒ 1.85 回転/sec
303 Hz / 1.85 回転 / sec ≒ 164 パルス / 回転
右モーター
(測定データ)10.0 sec / 20 回転、324 Hz , 315 Hz (平均 320 Hz) 右モーターの方が左モーターより回転数が多い。
20 回転 / 10.0 sec ≒ 2.00 回転/sec
320 Hz / 2.00 回転 / sec = 160 パルス / 回転
// DMA_Read_addr.cpp
// 2016/07/13 by marsee
//
// frame_buffer0, frame_buffer1, frame_buffer2 には3つのフレームバッファのアドレスを入れる
// mode = 0 : DMA Write IP の active_frame を見て、その1つ前のフレームをDMA Readするモード(DMA_WRITE_MODE)
// mode = 1 : フリーラン モード(FREE_RUN_MODE)
#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include "DMA_Read.h"
int DMA_Read_addr(volatile int *in, hls::stream<ap_axis<32,1,1,1> >& outs,
unsigned int frame_buffer0, unsigned int frame_buffer1,
unsigned int frame_buffer2, ap_uint<2> & active_frame,
ap_uint<1> mode){
#pragma HLS INTERFACE s_axilite port=mode
#pragma HLS INTERFACE ap_none port=active_frame
#pragma HLS INTERFACE s_axilite port=frame_buffer0
#pragma HLS INTERFACE s_axilite port=frame_buffer1
#pragma HLS INTERFACE s_axilite port=frame_buffer2
#pragma HLS INTERFACE m_axi depth=5000000 port=in offset=off
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return
ap_axis<32,1,1,1> pix;
unsigned int dma_index, n;
for (int i=0; i<MAX_FRAME_NUMBER; i++){
if (mode == DMA_WRITE_MODE)
n = (unsigned int)active_frame;
else
n = (unsigned int)i;
switch (n){ // 1つ前のフレームバッファを読みだす
case 0 :
dma_index = frame_buffer2/sizeof(int);
break;
case 1 :
dma_index = frame_buffer0/sizeof(int);
break;
case 2 :
dma_index = frame_buffer1/sizeof(int);
break;
default :
dma_index = frame_buffer0/sizeof(int);
break;
}
for (int y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (int x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
pix.data = in[dma_index+(y*HORIZONTAL_PIXEL_WIDTH)+x];
if (y==0 && x==0)
pix.user = 1;
else
pix.user = 0;
if (x == (HORIZONTAL_PIXEL_WIDTH-1))
pix.last = 1;
else
pix.last = 0;
outs << pix;
}
}
}
return 0;
}
span class="src_singlelinecomment">// DMA_Read.h
// 2016/07/14 by marsee
//
//#define HORIZONTAL_PIXEL_WIDTH 800
//#define VERTICAL_PIXEL_WIDTH 600
#define HORIZONTAL_PIXEL_WIDTH 64
#define VERTICAL_PIXEL_WIDTH 48
#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)
#define MAX_FRAME_NUMBER 3
#define DMA_WRITE_MODE 0
#define FREE_RUN_MODE 1
// DMA_Read_addr_tb.cpp
// 2016/07/15 by marsee
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <ap_axi_sdata.h>
#include "DMA_Read.h"
#include "bmp_header.h"
int DMA_Read_addr(volatile int *in, hls::stream<ap_axis<32,1,1,1> >& outs,
unsigned int frame_buffer0, unsigned int frame_buffer1,
unsigned int frame_buffer2, ap_uint<2> & active_frame,
ap_uint<1> mode);
int main()
{
using namespace std;
hls::stream<ap_axis<32,1,1,1> > outs_dummy;
hls::stream<ap_axis<32,1,1,1> > outs;
ap_axis<32,1,1,1> pix;
ap_axis<32,1,1,1> vals;
BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
FILE *fbmpr, *fbmpw;
int *rd_bmp, *hw_lapd;
int blue, green, red;
ap_uint<2> active_frame = 0;
int *frame_buffer;
if ((fbmpr = fopen("test.bmp", "rb")) == NULL){ // test.bmp をオープン
fprintf(stderr, "Can't open test.bmp by binary read mode\n");
exit(1);
}
// bmpヘッダの読み出し
fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);
// ピクセルを入れるメモリをアロケートする
if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate rd_bmp memory\n");
exit(1);
}
int *buf;
if ((buf =(int *)malloc(3 * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate buf memory\n");
exit(1);
}
// rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = fgetc(fbmpr);
green = fgetc(fbmpr);
red = fgetc(fbmpr);
rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
}
}
fclose(fbmpr);
// frame buffer をアロケートする、3倍の領域を取ってそれを3つに分ける
if ((frame_buffer =(int *)malloc(MAX_FRAME_NUMBER * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate frame_buffer0 ~ 2\n");
exit(1);
}
// 3 つのフレームバッファにそれぞれ'A' を入力する
memcpy(frame_buffer, rd_bmp, bmpihr.biHeight * bmpihr.biWidth * sizeof(int));
memcpy((int *)((unsigned int)frame_buffer + bmpihr.biHeight * bmpihr.biWidth * sizeof(int)),
rd_bmp, bmpihr.biHeight * bmpihr.biWidth * sizeof(int));
memcpy((int *)((unsigned int)frame_buffer + 2 * bmpihr.biHeight * bmpihr.biWidth * sizeof(int)),
rd_bmp, bmpihr.biHeight * bmpihr.biWidth * sizeof(int));
DMA_Read_addr((volatile int *)0, outs_dummy, (unsigned int)frame_buffer,
(unsigned int)frame_buffer+(bmpihr.biWidth * bmpihr.biHeight * sizeof(int)),
(unsigned int)frame_buffer+(2 * (bmpihr.biWidth * bmpihr.biHeight) * sizeof(int)),
active_frame, DMA_WRITE_MODE);
DMA_Read_addr((volatile int *)0, outs, (unsigned int)frame_buffer,
(unsigned int)frame_buffer+(bmpihr.biWidth * bmpihr.biHeight * sizeof(int)),
(unsigned int)frame_buffer+(2 * (bmpihr.biWidth * bmpihr.biHeight) * sizeof(int)),
active_frame, FREE_RUN_MODE);
// outs ストリームのデータを buf に入力する
for (int k=0; k<3; k++){
for(int j=0; j < bmpihr.biHeight; j++){
for(int i=0; i < bmpihr.biWidth; i++){
outs >> vals;
ap_int<32> val = vals.data;
buf[(k*bmpihr.biWidth*bmpihr.biHeight)+(j*bmpihr.biWidth)+i] = (int)val;
}
}
}
// DMAされたデータをBMPフィルに書き込む
char output_file[] = "dma_result0.bmp";
for (int i=0; i<MAX_FRAME_NUMBER; i++){
switch (i){
case 0:
strcpy(output_file,"dma_result0.bmp");
break;
case 1:
strcpy(output_file,"dma_result1.bmp");
break;
case 2:
strcpy(output_file,"dma_result2.bmp");
break;
}
if ((fbmpw=fopen(output_file, "wb")) == NULL){
fprintf(stderr, "Can't open %s by binary write mode\n", output_file);
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
// RGB データの書き込み、逆順にする
int offset = i * bmpihr.biWidth * bmpihr.biHeight;
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = buf[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (buf[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (buf[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpw);
fputc(green, fbmpw);
fputc(red, fbmpw);
}
}
fclose(fbmpw);
}
free(rd_bmp);
free(frame_buffer);
return 0;
}
// zybot_motor.cpp
// 2016/07/07 by marsee
//
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <string.h>
#include "xpwm.h"
#include "xmotor_monitor.h"
void motor_settings(XPwm *motorLp, XPwm *motorRp){
XPwm_DisableAutoRestart(motorLp);
while(!XPwm_IsIdle(motorLp)) ;
XPwm_Start(motorLp);
XPwm_EnableAutoRestart(motorLp);
XPwm_DisableAutoRestart(motorRp);
while(!XPwm_IsIdle(motorRp)) ;
XPwm_Start(motorRp);
XPwm_EnableAutoRestart(motorRp);
}
void Stopped_Zybot(XPwm *motorLp, XPwm *motorRp){
XPwm_Set_sw_late_V(motorLp, 0);
XPwm_Set_sw_late_V(motorRp, 0);
}
int main(){
XPwm motorL, motorR;
XPwm *motorLp, *motorRp;
XMotor_monitor mmL, mmR;
XMotor_monitor *mmLp, *mmRp;
int c = 0;
int motorvalL, motorvalR;
int mmonitorL, mmonitorR;
motorLp = &motorL;
motorRp = &motorR;
mmLp = &mmL;
mmRp = &mmR;
// Initialization of motor
if (XPwm_Initialize(motorLp, "pwm_0") != XST_SUCCESS){
fprintf(stderr,"pwm_0 (Left) open error\n");
exit(-1);
}
if (XPwm_Initialize(motorRp, "pwm_1") != XST_SUCCESS){
fprintf(stderr,"pwm_1 (Right) open error\n");
exit(-1);
}
// Initialization of motor monitor
if (XMotor_monitor_Initialize(mmLp, "motor_monitor_0") != XST_SUCCESS){
fprintf(stderr,"motor_monitor_0 (Left) open error\n");
exit(-1);
}
if (XMotor_monitor_Initialize(mmRp, "motor_monitor_1") != XST_SUCCESS){
fprintf(stderr,"motor_monitor_1 (Right) open error\n");
exit(-1);
}
// The Motors is rotated in the forward direction.
XPwm_Set_sw_late_V(&motorL, 0);
XPwm_Set_dir_V(&motorL, 1);
XPwm_Set_sw_late_V(&motorR, 0);
XPwm_Set_dir_V(&motorR, 0);
motor_settings(motorLp, motorRp);
// main loop
printf("Zybot Motor Control Application Start...\n");
while(c != 'q'){
c = getc(stdin);
switch ((char)c) {
case 'l':{ // Left motor value setting
printf("Please input Left motor value = ");
scanf("%d", &c);
motorvalL = c;
break;
}
case 'r':{ // Right motor value setting
printf("Please input Right motor value = ");
scanf("%d", &c);
motorvalR = c;
break;
}
case 't':{ // To change the power of the right and left motor.
printf("Left motor = %d, Right motor = %d\n", motorvalL, motorvalR);
XPwm_Set_sw_late_V(&motorL, motorvalL);
XPwm_Set_sw_late_V(&motorR, motorvalR);
break;
}
case 'm':{ // Enter the difference between the motor value.
printf("motor value = L %d, R %d\n", motorvalL, motorvalR);
printf("Please input Motor output difference = ");
scanf("%d", &c);
motorvalL = motorvalL + c;
motorvalR = motorvalR + c;
if (motorvalL < 0)
motorvalL = 0;
if (motorvalR < 0)
motorvalR = 0;
if (motorvalL > 100)
motorvalL = 100;
if (motorvalR > 100)
motorvalR = 100;
XPwm_Set_sw_late_V(&motorL, motorvalL);
XPwm_Set_sw_late_V(&motorR, motorvalR);
break;
}
case 'a':{ // The Motors is rotated in the forward or reverse direction.
if (XPwm_Get_dir_V(&motorL)){ // forward
Stopped_Zybot(motorLp, motorRp);
sleep(1);
XPwm_Set_sw_late_V(&motorL, motorvalL);
XPwm_Set_sw_late_V(&motorR, motorvalR);
XPwm_Set_dir_V(&motorL, 0); // reverse
XPwm_Set_dir_V(&motorR, 1);
}else{
Stopped_Zybot(motorLp, motorRp);
sleep(1);
XPwm_Set_sw_late_V(&motorL, motorvalL);
XPwm_Set_sw_late_V(&motorR, motorvalR);
XPwm_Set_dir_V(&motorL, 1); // forward
XPwm_Set_dir_V(&motorR, 0);
}
break;
}
case 'd':{ // Displayed the motor monitor value
while (!XMotor_monitor_IsIdle(&mmL));
XMotor_monitor_Start(&mmL);
while (!XMotor_monitor_IsIdle(&mmR));
XMotor_monitor_Start(&mmR);
while (!XMotor_monitor_IsIdle(&mmL));
while (!XMotor_monitor_IsIdle(&mmR));
unsigned int sa_countL = (unsigned int)XMotor_monitor_Get_sa_count_V(&mmL);
unsigned int sa_countR = (unsigned int)XMotor_monitor_Get_sa_count_V(&mmR);
unsigned int sb_levelL = (unsigned int)XMotor_monitor_Get_sb_level_V(&mmL);
unsigned int sb_levelR = (unsigned int)XMotor_monitor_Get_sb_level_V(&mmR);
unsigned int returnL = (unsigned int)XMotor_monitor_Get_return(&mmL);
unsigned int returnR = (unsigned int)XMotor_monitor_Get_return(&mmR);
unsigned int sum_cntL = (unsigned int)XMotor_monitor_Get_sum_count(&mmL);
unsigned int sum_cntR = (unsigned int)XMotor_monitor_Get_sum_count(&mmR);
printf("sa_countL = %d, sb_levelL = %d, retrunL = %d, sum_cntL = %d\n", sa_countL, sb_levelL, returnL, sum_cntL);
printf("freqency = %dHz, peirod = %fms\n", (int)(1/((float)sa_countL/1000000)), (float)sa_countL/1000);
printf("sa_countR = %d, sb_levelR = %d, retrunR = %d, sum_cntR = %d\n", sa_countR, sb_levelR, returnR, sum_cntR);
printf("freqency = %dHz, peirod = %fms\n", (int)(1/((float)sa_countR/1000000)), (float)sa_countR/1000);
break;
}
case 's':{ // Stop
Stopped_Zybot(motorLp, motorRp);
motorvalL = 0;
motorvalR = 0;
break;
}
case 'h':{ // help
printf("l: Left motor value setting\n");
printf("r: Right motor value setting\n");
printf("t: To change the power of the right and left motor\n");
printf("m: Enter the difference between the motor value. eg. 10, -10\n");
printf("a: The Motors is rotated in the forward or reverse direction. Altarnative\n");
printf("d: Displayed the motor monitor value\n");
printf("s: Stop\n");
printf("q: Quit\n");
break;
}
}
}
return 0;
}
#Makefile
# Referred to http://www.ie.u-ryukyu.ac.jp/~e085739/c.makefile.tuts.html
PROGRAM = zybot_motor
OBJS = zybot_motor.o xpwm_linux.o xpwm.o xmotor_monitor_linux.o xmotor_monitor.o
CC = gcc
CFLAGS = -Wall -O2
.SUFFIXES: .c .o
.PHONY: all
all: zybot_motor
zybot_motor: $(OBJS)
$(CC) -Wall -o $@ $(OBJS)
.c.o:
$(CC) $(CFLAGS) -c $<
.PHONY: clean
clean:
$(RM) $(PROGRAM) $(OBJS)
// DMA_Write.cpp
// 2016/07/10 by marsee
//
// fb0_offset_addr, fb1_offset_addr, fb2_offset_addr にはベースアドレスからのオフセット・アドレスを入れる
//
#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include "DMA_Write.h"
int DMA_Write(hls::stream<ap_axis<32,1,1,1> >& ins, volatile int *out,
unsigned int fb0_offset_addr, unsigned int fb1_offset_addr,
unsigned int fb2_offset_addr, volatile ap_uint<2> & active_frame){
#pragma HLS INTERFACE ap_vld port=active_frame
#pragma HLS INTERFACE s_axilite port=fb0_offset_addr
#pragma HLS INTERFACE s_axilite port=fb1_offset_addr
#pragma HLS INTERFACE s_axilite port=fb2_offset_addr
#pragma HLS INTERFACE m_axi depth=9216 port=out offset=slave
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE s_axilite port=return
ap_axis<32,1,1,1> pix;
int dma_index;
for (int i=0; i<MAX_FRAME_NUMBER; i++){
switch (i){
case 0 :
dma_index = fb0_offset_addr/sizeof(int);
break;
case 1 :
dma_index = fb1_offset_addr/sizeof(int);
break;
case 2 :
dma_index = fb2_offset_addr/sizeof(int);
break;
}
active_frame = i;
do { // user が 1になった時にフレームがスタートする
#pragma HLS LOOP_TRIPCOUNT min=1 max=1 avg=1
ins >> pix;
} while(pix.user == 0);
for (int y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (int x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
if (!(x==0 && y==0)) // 最初の入力はすでに入力されている
ins >> pix; // AXI4-Stream からの入力
out[dma_index+(y*HORIZONTAL_PIXEL_WIDTH)+x] = pix.data;
}
}
}
return 0;
}
次に、DMA_Write.h を示す。
// DMA_Write.h
// 2016/07/10 by marsee
//
//#define HORIZONTAL_PIXEL_WIDTH 800
//#define VERTICAL_PIXEL_WIDTH 600
#define HORIZONTAL_PIXEL_WIDTH 64
#define VERTICAL_PIXEL_WIDTH 48
#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)
#define MAX_FRAME_NUMBER 3
// DMA_Write_tb.cpp
// 2016/07/10 by marsee
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <ap_axi_sdata.h>
#include "DMA_Write.h"
#include "bmp_header.h"
int DMA_Write(hls::stream<ap_axis<32,1,1,1> >& ins, volatile int *out,
unsigned int fb0_offset_addr, unsigned int fb1_offset_addr,
unsigned int fb2_offset_addr, volatile ap_uint<2> & active_frame);
int main()
{
using namespace std;
hls::stream<ap_axis<32,1,1,1> > ins;
ap_axis<32,1,1,1> pix;
BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
FILE *fbmpr, *fbmpw;
int *rd_bmp, *hw_lapd;
int blue, green, red;
ap_uint<2> active_frame;
int *frame_buffer;
if ((fbmpr = fopen("test.bmp", "rb")) == NULL){ // test.bmp をオープン
fprintf(stderr, "Can't open test.bmp by binary read mode\n");
exit(1);
}
// bmpヘッダの読み出し
fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);
// ピクセルを入れるメモリをアロケートする
if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate rd_bmp memory\n");
exit(1);
}
// rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = fgetc(fbmpr);
green = fgetc(fbmpr);
red = fgetc(fbmpr);
rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
}
}
fclose(fbmpr);
// ins に入力データを用意する
for(int i=0; i<5; i++){ // dummy data
pix.user = 0;
pix.data = i;
ins << pix;
}
for(int k=0; k<MAX_FRAME_NUMBER; k++){
for(int j=0; j < bmpihr.biHeight; j++){
for(int i=0; i < bmpihr.biWidth; i++){
pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];
if (j==0 && i==0) // 最初のデータの時に TUSER を 1 にする
pix.user = 1;
else
pix.user = 0;
if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
pix.last = 1;
else
pix.last = 0;
ins << pix;
}
}
}
// frame buffer をアロケートする、3倍の領域を取ってそれを3つに分ける
if ((frame_buffer =(int *)malloc(MAX_FRAME_NUMBER * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate frame_buffer0 ~ 2\n");
exit(1);
}
DMA_Write(ins, (volatile int *)frame_buffer, (unsigned int)0,
(unsigned int)(bmpihr.biWidth * bmpihr.biHeight * sizeof(int)),
(unsigned int)(2 * (bmpihr.biWidth * bmpihr.biHeight) * sizeof(int)),
active_frame);
// DMAされたデータをBMPフィルに書き込む
char output_file[] = "dma_result0.bmp";
for (int i=0; i<MAX_FRAME_NUMBER; i++){
switch (i){
case 0:
strcpy(output_file,"dma_result0.bmp");
break;
case 1:
strcpy(output_file,"dma_result1.bmp");
break;
case 2:
strcpy(output_file,"dma_result2.bmp");
break;
}
if ((fbmpw=fopen(output_file, "wb")) == NULL){
fprintf(stderr, "Can't open %s by binary write mode\n", output_file);
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
// RGB データの書き込み、逆順にする
int offset = i * bmpihr.biWidth * bmpihr.biHeight;
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpw);
fputc(green, fbmpw);
fputc(red, fbmpw);
}
}
fclose(fbmpw);
}
free(rd_bmp);
free(frame_buffer);
return 0;
}
//------------------------Address Info-------------------
// 0x00 : Control signals
// bit 0 - ap_start (Read/Write/COH)
// bit 1 - ap_done (Read/COR)
// bit 2 - ap_idle (Read)
// bit 3 - ap_ready (Read)
// bit 7 - auto_restart (Read/Write)
// others - reserved
// 0x04 : Global Interrupt Enable Register
// bit 0 - Global Interrupt Enable (Read/Write)
// others - reserved
// 0x08 : IP Interrupt Enable Register (Read/Write)
// bit 0 - Channel 0 (ap_done)
// bit 1 - Channel 1 (ap_ready)
// others - reserved
// 0x0c : IP Interrupt Status Register (Read/TOW)
// bit 0 - Channel 0 (ap_done)
// bit 1 - Channel 1 (ap_ready)
// others - reserved
// 0x10 : Data signal of ap_return
// bit 31~0 - ap_return[31:0] (Read)
// 0x18 : Data signal of out_r
// bit 31~0 - out_r[31:0] (Read/Write)
// 0x1c : reserved
// 0x20 : Data signal of fb0_offset_addr
// bit 31~0 - fb0_offset_addr[31:0] (Read/Write)
// 0x24 : reserved
// 0x28 : Data signal of fb1_offset_addr
// bit 31~0 - fb1_offset_addr[31:0] (Read/Write)
// 0x2c : reserved
// 0x30 : Data signal of fb2_offset_addr
// bit 31~0 - fb2_offset_addr[31:0] (Read/Write)
// 0x34 : reserved
// (SC = Self Clear, COR = Clear on Read, TOW = Toggle on Write, COH = Clear on Handshake)
// DMA_Write.cpp
// 2016/07/10 by marsee
//
// frame_buffer0, frame_buffer1, frame_buffer2 には3つのフレームバッファのアドレスを入れる
//
#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include "DMA_Write.h"
int DMA_Write(hls::stream<ap_axis<32,1,1,1> >& ins, volatile int *out,
unsigned int frame_buffer0, unsigned int frame_buffer1,
unsigned int frame_buffer2, volatile ap_uint<2> & active_frame){
#pragma HLS INTERFACE ap_vld port=active_frame
#pragma HLS INTERFACE s_axilite port=frame_buffer0
#pragma HLS INTERFACE s_axilite port=frame_buffer1
#pragma HLS INTERFACE s_axilite port=frame_buffer2
#pragma HLS INTERFACE m_axi depth=5000000 port=out offset=off
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE s_axilite port=return
ap_axis<32,1,1,1> pix;
int dma_index;
for (int i=0; i<MAX_FRAME_NUMBER; i++){
switch (i){
case 0 :
dma_index = frame_buffer0/sizeof(int);
break;
case 1 :
dma_index = frame_buffer1/sizeof(int);
break;
case 2 :
dma_index = frame_buffer2/sizeof(int);
break;
}
active_frame = i;
do { // user が 1になった時にフレームがスタートする
#pragma HLS LOOP_TRIPCOUNT min=1 max=1 avg=1
ins >> pix;
} while(pix.user == 0);
for (int y=0; y<VERTICAL_PIXEL_WIDTH; y++){
for (int x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
if (!(x==0 && y==0)) // 最初の入力はすでに入力されている
ins >> pix; // AXI4-Stream からの入力
out[dma_index+(y*HORIZONTAL_PIXEL_WIDTH)+x] = pix.data;
}
}
}
return 0;
}
// DMA_Write.h
// 2016/07/10 by marsee
//
//#define HORIZONTAL_PIXEL_WIDTH 800
//#define VERTICAL_PIXEL_WIDTH 600
#define HORIZONTAL_PIXEL_WIDTH 64
#define VERTICAL_PIXEL_WIDTH 48
#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)
#define MAX_FRAME_NUMBER 3
// DMA_Write_tb.cpp
// 2016/07/10 by marsee
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <ap_axi_sdata.h>
#include "DMA_Write.h"
#include "bmp_header.h"
int DMA_Write(hls::stream<ap_axis<32,1,1,1> >& ins, volatile int *out,
unsigned int fb0_offset_addr, unsigned int fb1_offset_addr,
unsigned int fb2_offset_addr, volatile ap_uint<2> & active_frame);
int main()
{
using namespace std;
hls::stream<ap_axis<32,1,1,1> > ins;
ap_axis<32,1,1,1> pix;
BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
FILE *fbmpr, *fbmpw;
int *rd_bmp, *hw_lapd;
int blue, green, red;
ap_uint<2> active_frame;
int *frame_buffer;
if ((fbmpr = fopen("test.bmp", "rb")) == NULL){ // test.bmp をオープン
fprintf(stderr, "Can't open test.bmp by binary read mode\n");
exit(1);
}
// bmpヘッダの読み出し
fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);
// ピクセルを入れるメモリをアロケートする
if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate rd_bmp memory\n");
exit(1);
}
// rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = fgetc(fbmpr);
green = fgetc(fbmpr);
red = fgetc(fbmpr);
rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
}
}
fclose(fbmpr);
// ins に入力データを用意する
for(int i=0; i<5; i++){ // dummy data
pix.user = 0;
pix.data = i;
ins << pix;
}
for(int k=0; k<MAX_FRAME_NUMBER; k++){
for(int j=0; j < bmpihr.biHeight; j++){
for(int i=0; i < bmpihr.biWidth; i++){
pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];
if (j==0 && i==0) // 最初のデータの時に TUSER を 1 にする
pix.user = 1;
else
pix.user = 0;
if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
pix.last = 1;
else
pix.last = 0;
ins << pix;
}
}
}
// frame buffer をアロケートする、3倍の領域を取ってそれを3つに分ける
if ((frame_buffer =(int *)malloc(MAX_FRAME_NUMBER * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
fprintf(stderr, "Can't allocate frame_buffer0 ~ 2\n");
exit(1);
}
DMA_Write(ins, (volatile int *)frame_buffer, (unsigned int)0,
(unsigned int)(bmpihr.biWidth * bmpihr.biHeight * sizeof(int)),
(unsigned int)(2 * (bmpihr.biWidth * bmpihr.biHeight) * sizeof(int)),
active_frame);
// DMAされたデータをBMPフィルに書き込む
char output_file[] = "dma_result0.bmp";
for (int i=0; i<MAX_FRAME_NUMBER; i++){
switch (i){
case 0:
strcpy(output_file,"dma_result0.bmp");
break;
case 1:
strcpy(output_file,"dma_result1.bmp");
break;
case 2:
strcpy(output_file,"dma_result2.bmp");
break;
}
if ((fbmpw=fopen(output_file, "wb")) == NULL){
fprintf(stderr, "Can't open %s by binary write mode\n", output_file);
exit(1);
}
// BMPファイルヘッダの書き込み
fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
// RGB データの書き込み、逆順にする
int offset = i * bmpihr.biWidth * bmpihr.biHeight;
for (int y=0; y<bmpihr.biHeight; y++){
for (int x=0; x<bmpihr.biWidth; x++){
blue = frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
green = (frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
red = (frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;
fputc(blue, fbmpw);
fputc(green, fbmpw);
fputc(red, fbmpw);
}
}
fclose(fbmpw);
}
free(rd_bmp);
free(frame_buffer);
return 0;
}
//------------------------Address Info-------------------
// 0x00 : Control signals
// bit 0 - ap_start (Read/Write/COH)
// bit 1 - ap_done (Read/COR)
// bit 2 - ap_idle (Read)
// bit 3 - ap_ready (Read)
// bit 7 - auto_restart (Read/Write)
// others - reserved
// 0x04 : Global Interrupt Enable Register
// bit 0 - Global Interrupt Enable (Read/Write)
// others - reserved
// 0x08 : IP Interrupt Enable Register (Read/Write)
// bit 0 - Channel 0 (ap_done)
// bit 1 - Channel 1 (ap_ready)
// others - reserved
// 0x0c : IP Interrupt Status Register (Read/TOW)
// bit 0 - Channel 0 (ap_done)
// bit 1 - Channel 1 (ap_ready)
// others - reserved
// 0x10 : Data signal of ap_return
// bit 31~0 - ap_return[31:0] (Read)
// 0x18 : Data signal of frame_buffer0
// bit 31~0 - frame_buffer0[31:0] (Read/Write)
// 0x1c : reserved
// 0x20 : Data signal of frame_buffer1
// bit 31~0 - frame_buffer1[31:0] (Read/Write)
// 0x24 : reserved
// 0x28 : Data signal of frame_buffer2
// bit 31~0 - frame_buffer2[31:0] (Read/Write)
// 0x2c : reserved
// (SC = Self Clear, COR = Clear on Read, TOW = Toggle on Write, COH = Clear on Handshake)
-----------------------------------------------------------------------------
--
-- AXI Master用 Slave Bus Function Mode (BFM)
-- axi_slave_BFM.vhd
--
-----------------------------------------------------------------------------
-- 2012/02/25 : S_AXI_AWBURST=1 (INCR) にのみ対応、AWSIZE, ARSIZE = 000 (1byte), 001 (2bytes), 010 (4bytes) のみ対応。
-- 2012/07/04 : READ_ONLY_TRANSACTION を追加。Read機能のみでも+1したデータを出力することが出来るように変更した。
-- sync_fifo を使用したオーバーラップ対応版
-- 2014/07/04 : M_AXIをスレーブに対応した名前のS_AXIに変更
-- 2014/07/16 : Write Respose Channel に sync_fifo を使用した
-- 2014/08/31 : READ_RANDOM_WAIT=1 の時に、S_AXI_RREADY が S_AXI_RVALID に依存するバグをフィック。
-- WRITE_RANDOM_WAIT=1 の時に、S_AXI_WVALID が S_AXI_WREADY に依存するバグをフィック。
--
-- 2016/07/03 : AWREADY_IS_USUALLY_HIGH と ARREADY_IS_USUALLY_HIGH の2つのパラメータを追加 by marsee
--
-- ライセンスは二条項BSDライセンス (2-clause BSD license)とします。
--
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.std_logic_misc.all;
package m_seq_bfm_pack is
function M_SEQ16_BFM_F(mseq16in : std_logic_vector
)return std_logic_vector;
end package m_seq_bfm_pack;
package body m_seq_bfm_pack is
function M_SEQ16_BFM_F(mseq16in : std_logic_vector
)return std_logic_vector is
variable mseq16 : std_logic_vector(15 downto 0);
variable xor_result : std_logic;
begin
xor_result := mseq16in(15) xor mseq16in(12) xor mseq16in(10) xor mseq16in(8) xor mseq16in(7) xor mseq16in(6) xor mseq16in(3) xor mseq16in(2);
mseq16 := mseq16in(14 downto 0) & xor_result;
return mseq16;
end M_SEQ16_BFM_F;
end m_seq_bfm_pack;
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.std_logic_misc.all;
use IEEE.math_real.all;
library work;
use work.m_seq_bfm_pack.all;
--library unisim;
--use unisim.vcomponents.all;
entity axi_slave_bfm is
generic (
C_S_AXI_ID_WIDTH : integer := 1;
C_S_AXI_ADDR_WIDTH : integer := 32;
C_S_AXI_DATA_WIDTH : integer := 32;
C_S_AXI_AWUSER_WIDTH : integer := 1;
C_S_AXI_ARUSER_WIDTH : integer := 1;
C_S_AXI_WUSER_WIDTH : integer := 1;
C_S_AXI_RUSER_WIDTH : integer := 1;
C_S_AXI_BUSER_WIDTH : integer := 1;
C_S_AXI_TARGET : integer := 0;
C_OFFSET_WIDTH : integer := 10; -- 割り当てるRAMのアドレスのビット幅
C_S_AXI_BURST_LEN : integer := 256;
WRITE_RANDOM_WAIT : integer := 1; -- Write Transaction のデータ転送の時にランダムなWaitを発生させる=1, Waitしない=0
READ_RANDOM_WAIT : integer := 0; -- Read Transaction のデータ転送の時にランダムなWaitを発生させる=1, Waitしない=0
READ_DATA_IS_INCREMENT : integer := 0; -- ReadトランザクションでRAMの内容をReadする = 0(RAMにWriteしたものをReadする)、Readデータを+1する = 1(データは+1したデータをReadデータとして使用する
RANDOM_BVALID_WAIT : integer := 0; -- Write Data Transaction が終了した後で、BVALID をランダムにWaitする = 1、BVALID をランダムにWaitしない = 0, 31 ~ 0 クロックのWait
AWREADY_IS_USUALLY_HIGH : integer := 1; -- AWRAEDY は通常はLow=0, High=1
ARREADY_IS_USUALLY_HIGH : integer := 1 -- AWRAEDY は通常はLow=0, High=1
);
port(
-- System Signals
ACLK : in std_logic;
ARESETN : in std_logic;
-- Master Interface Write Address Ports
S_AXI_AWID : in std_logic_vector(C_S_AXI_ID_WIDTH-1 downto 0);
S_AXI_AWADDR : in std_logic_vector(C_S_AXI_ADDR_WIDTH-1 downto 0);
S_AXI_AWLEN : in std_logic_vector(8-1 downto 0);
S_AXI_AWSIZE : in std_logic_vector(3-1 downto 0);
S_AXI_AWBURST : in std_logic_vector(2-1 downto 0);
-- S_AXI_AWLOCK : in std_logic_vector(2-1 downto 0);
S_AXI_AWLOCK : in std_logic_vector(1 downto 0);
S_AXI_AWCACHE : in std_logic_vector(4-1 downto 0);
S_AXI_AWPROT : in std_logic_vector(3-1 downto 0);
S_AXI_AWQOS : in std_logic_vector(4-1 downto 0);
S_AXI_AWUSER : in std_logic_vector(C_S_AXI_AWUSER_WIDTH-1 downto 0);
S_AXI_AWVALID : in std_logic;
S_AXI_AWREADY : out std_logic;
-- Master Interface Write Data Ports
S_AXI_WDATA : in std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
S_AXI_WSTRB : in std_logic_vector(C_S_AXI_DATA_WIDTH/8-1 downto 0);
S_AXI_WLAST : in std_logic;
S_AXI_WUSER : in std_logic_vector(C_S_AXI_WUSER_WIDTH-1 downto 0);
S_AXI_WVALID : in std_logic;
S_AXI_WREADY : out std_logic;
-- Master Interface Write Response Ports
S_AXI_BID : out std_logic_vector(C_S_AXI_ID_WIDTH-1 downto 0);
S_AXI_BRESP : out std_logic_vector(2-1 downto 0);
S_AXI_BUSER : out std_logic_vector(C_S_AXI_BUSER_WIDTH-1 downto 0);
S_AXI_BVALID : out std_logic;
S_AXI_BREADY : in std_logic;
-- Master Interface Read Address Ports
S_AXI_ARID : in std_logic_vector(C_S_AXI_ID_WIDTH-1 downto 0);
S_AXI_ARADDR : in std_logic_vector(C_S_AXI_ADDR_WIDTH-1 downto 0);
S_AXI_ARLEN : in std_logic_vector(8-1 downto 0);
S_AXI_ARSIZE : in std_logic_vector(3-1 downto 0);
S_AXI_ARBURST : in std_logic_vector(2-1 downto 0);
S_AXI_ARLOCK : in std_logic_vector(2-1 downto 0);
S_AXI_ARCACHE : in std_logic_vector(4-1 downto 0);
S_AXI_ARPROT : in std_logic_vector(3-1 downto 0);
S_AXI_ARQOS : in std_logic_vector(4-1 downto 0);
S_AXI_ARUSER : in std_logic_vector(C_S_AXI_ARUSER_WIDTH-1 downto 0);
S_AXI_ARVALID : in std_logic;
S_AXI_ARREADY : out std_logic;
-- Master Interface Read Data Ports
S_AXI_RID : out std_logic_vector(C_S_AXI_ID_WIDTH-1 downto 0);
S_AXI_RDATA : out std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
S_AXI_RRESP : out std_logic_vector(2-1 downto 0);
S_AXI_RLAST : out std_logic;
S_AXI_RUSER : out std_logic_vector(C_S_AXI_RUSER_WIDTH-1 downto 0);
S_AXI_RVALID : out std_logic;
S_AXI_RREADY : in std_logic
);
end axi_slave_bfm;
architecture implementation of axi_slave_bfm is
constant AxBURST_FIXED : std_logic_vector := "00";
constant AxBURST_INCR : std_logic_vector := "01";
constant AxBURST_WRAP : std_logic_vector := "10";
constant RESP_OKAY : std_logic_vector := "00";
constant RESP_EXOKAY : std_logic_vector := "01";
constant RESP_SLVERR : std_logic_vector := "10";
constant RESP_DECERR : std_logic_vector := "11";
constant DATA_BUS_BYTES : natural := C_S_AXI_DATA_WIDTH/8; -- データバスのビット幅
constant ADD_INC_OFFSET : natural := natural(log(real(DATA_BUS_BYTES), 2.0));
-- fifo depth for address
constant AD_FIFO_DEPTH : natural := 16;
-- wad_fifo field
constant WAD_FIFO_WIDTH : natural := C_S_AXI_ADDR_WIDTH+5+C_S_AXI_ID_WIDTH-1+1;
constant WAD_FIFO_AWID_HIGH : natural := C_S_AXI_ADDR_WIDTH+5+C_S_AXI_ID_WIDTH-1;
constant WAD_FIFO_AWID_LOW : natural := C_S_AXI_ADDR_WIDTH+5;
constant WAD_FIFO_AWBURST_HIGH : natural := C_S_AXI_ADDR_WIDTH+4;
constant WAD_FIFO_AWBURST_LOW : natural := C_S_AXI_ADDR_WIDTH+3;
constant WAD_FIFO_AWSIZE_HIGH : natural := C_S_AXI_ADDR_WIDTH+2;
constant WAD_FIFO_AWSIZE_LOW : natural := C_S_AXI_ADDR_WIDTH;
constant WAD_FIFO_ADDR_HIGH : natural := C_S_AXI_ADDR_WIDTH-1;
constant WAD_FIFO_ADDR_LOW : natural := 0;
-- wres_fifo field
constant WRES_FIFO_WIDTH : natural := 2+C_S_AXI_ID_WIDTH-1+1;
constant WRES_FIFO_AWID_HIGH : natural := 2+C_S_AXI_ID_WIDTH-1;
constant WRES_FIFO_AWID_LOW : natural := 2;
constant WRES_FIFO_AWBURST_HIGH : natural := 1;
constant WRES_FIFO_AWBURST_LOW : natural := 0;
-- rad_fifo field
constant RAD_FIFO_WIDTH : natural := C_S_AXI_ADDR_WIDTH+13+C_S_AXI_ID_WIDTH-1+1;
constant RAD_FIFO_ARID_HIGH : natural := C_S_AXI_ADDR_WIDTH+13+C_S_AXI_ID_WIDTH-1;
constant RAD_FIFO_ARID_LOW : natural := C_S_AXI_ADDR_WIDTH+13;
constant RAD_FIFO_ARBURST_HIGH : natural := C_S_AXI_ADDR_WIDTH+12;
constant RAD_FIFO_ARBURST_LOW : natural := C_S_AXI_ADDR_WIDTH+11;
constant RAD_FIFO_ARSIZE_HIGH : natural := C_S_AXI_ADDR_WIDTH+10;
constant RAD_FIFO_ARSIZE_LOW : natural := C_S_AXI_ADDR_WIDTH+8;
constant RAD_FIFO_ARLEN_HIGH : natural := C_S_AXI_ADDR_WIDTH+7;
constant RAD_FIFO_ARLEN_LOW : natural := C_S_AXI_ADDR_WIDTH;
constant RAD_FIFO_ADDR_HIGH : natural := C_S_AXI_ADDR_WIDTH-1;
constant RAD_FIFO_ADDR_LOW : natural := 0;
-- RAMの生成
constant SLAVE_ADDR_NUMBER : integer := 2**(C_OFFSET_WIDTH - ADD_INC_OFFSET);
type ram_array_def is array (SLAVE_ADDR_NUMBER-1 downto 0) of std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
signal ram_array : ram_array_def := (others => (others => '0'));
-- for write transaction
type write_address_state is (idle_wrad, awr_accept);
type write_data_state is (idle_wrdt, wr_burst);
type write_response_state is (idle_wres, wait_bvalid, bvalid_assert);
signal wradr_cs : write_address_state;
signal wrdat_cs : write_data_state;
signal wrres_cs : write_response_state;
signal addr_inc_step_wr : integer := 1;
signal awready : std_logic;
signal wr_addr : std_logic_vector(C_OFFSET_WIDTH-1 downto 0);
signal wr_bvalid : std_logic;
signal m_seq16_wr : std_logic_vector(15 downto 0);
signal wready : std_logic;
type wready_state is (idle_wready, assert_wready, deassert_wready);
signal cs_wready : wready_state;
signal cdc_we : std_logic;
signal wad_fifo_full, wad_fifo_empty : std_logic;
signal wad_fifo_almost_full, wad_fifo_almost_empty : std_logic;
signal wad_fifo_rd_en : std_logic;
signal wad_fifo_wr_en : std_logic;
signal wad_fifo_din : std_logic_vector(WAD_FIFO_WIDTH-1 downto 0);
signal wad_fifo_dout : std_logic_vector(WAD_FIFO_WIDTH-1 downto 0);
signal m_seq16_wr_res : std_logic_vector(15 downto 0);
signal wr_resp_cnt : std_logic_vector(4 downto 0);
signal wres_fifo_wr_en : std_logic;
signal wres_fifo_full, wres_fifo_empty : std_logic;
signal wres_fifo_almost_full, wres_fifo_almost_empty : std_logic;
signal wres_fifo_rd_en : std_logic;
signal wres_fifo_din : std_logic_vector(WRES_FIFO_WIDTH-1 downto 0);
signal wres_fifo_dout : std_logic_vector(WRES_FIFO_WIDTH-1 downto 0);
-- for read transaction
type read_address_state is (idle_rda, arr_accept);
type read_data_state is (idle_rdd, rd_burst);
type read_last_state is (idle_rlast, rlast_assert);
signal rdadr_cs : read_address_state;
signal rddat_cs : read_data_state;
signal rdlast : read_last_state;
signal addr_inc_step_rd : integer := 1;
signal arready : std_logic;
signal rd_addr : std_logic_vector(C_OFFSET_WIDTH-1 downto 0);
signal rd_axi_count : std_logic_vector(7 downto 0);
signal rvalid : std_logic;
signal rlast : std_logic;
signal m_seq16_rd : std_logic_vector(15 downto 0);
type rvalid_state is (idle_rvalid, assert_rvalid, deassert_rvalid);
signal cs_rvalid : rvalid_state;
signal read_data_count : std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
signal reset_1d, reset_2d, reset : std_logic := '1';
signal rad_fifo_full, rad_fifo_empty : std_logic;
signal rad_fifo_almost_full, rad_fifo_almost_empty : std_logic;
signal rad_fifo_rd_en : std_logic;
signal rad_fifo_wr_en : std_logic;
signal rad_fifo_din : std_logic_vector(RAD_FIFO_WIDTH-1 downto 0);
signal rad_fifo_dout : std_logic_vector(RAD_FIFO_WIDTH-1 downto 0);
component sync_fifo generic (
constant C_MEMORY_SIZE : integer := 512; -- Word (not byte), 2のn乗
constant DATA_BUS_WIDTH : integer := 32 -- RAM Data Width
);
port (
clk : in std_logic;
rst : in std_logic;
wr_en : in std_logic;
din : in std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
full : out std_logic;
almost_full : out std_logic;
rd_en : in std_logic;
dout : out std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
empty : out std_logic;
almost_empty : out std_logic
);
end component;
begin
-- ARESETN をACLK で同期化
process (ACLK) begin
if ACLK'event and ACLK='1' then
reset_1d <= not ARESETN;
reset_2d <= reset_1d;
end if;
end process;
reset <= reset_2d;
-- AXI4バス Write Address State Machine
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
wradr_cs <= idle_wrad;
awready <= '0';
else
case (wradr_cs) is
when idle_wrad =>
if S_AXI_AWVALID='1' and wad_fifo_full='0' and wres_fifo_full='0' then -- S_AXI_AWVALID が1にアサートされた
wradr_cs <= awr_accept;
awready <= '1';
end if;
when awr_accept => -- S_AXI_AWREADY をアサート
wradr_cs <= idle_wrad;
awready <= '0';
end case;
end if;
end if;
end process;
S_AXI_AWREADY <= not wad_fifo_full when AWREADY_IS_USUALLY_HIGH=1 else awready;
-- S_AXI_AWID & S_AXI_AWBURST & S_AXI_AWSIZE & S_AXI_AWADDR を保存しておく同期FIFO
wad_fifo_din <= (S_AXI_AWID & S_AXI_AWBURST & S_AXI_AWSIZE & S_AXI_AWADDR);
wad_fifo_wr_en <= (S_AXI_AWVALID and (not wad_fifo_full)) when AWREADY_IS_USUALLY_HIGH=1 else awready;
wad_fifo : sync_fifo generic map(
C_MEMORY_SIZE => AD_FIFO_DEPTH,
DATA_BUS_WIDTH => WAD_FIFO_WIDTH
) port map (
clk => ACLK,
rst => reset,
wr_en => wad_fifo_wr_en,
din => wad_fifo_din,
full => wad_fifo_full,
almost_full => wad_fifo_almost_full,
rd_en => wad_fifo_rd_en,
dout => wad_fifo_dout,
empty => wad_fifo_empty,
almost_empty => wad_fifo_almost_empty
);
wad_fifo_rd_en <= '1' when wready='1' and S_AXI_WVALID='1' and S_AXI_WLAST='1' else '0';
-- AXI4バス Write Data State Machine
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
wrdat_cs <= idle_wrdt;
else
case( wrdat_cs ) is
when idle_wrdt =>
if wad_fifo_empty='0' then -- AXI Write アドレス転送の残りが1個以上ある
wrdat_cs <= wr_burst;
end if;
when wr_burst => -- Writeデータの転送
if S_AXI_WLAST='1' and S_AXI_WVALID='1' and wready='1' then -- Write Transaction 終了
wrdat_cs <= idle_wrdt;
end if;
when others =>
end case ;
end if;
end if;
end process;
-- m_seq_wr、16ビットのM系列を計算する
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
m_seq16_wr <= (0 => '1', others => '0');
else
if WRITE_RANDOM_WAIT=1 then -- Write Transaction 時にランダムなWaitを挿入する
if wrdat_cs=wr_burst then
m_seq16_wr <= M_SEQ16_BFM_F(m_seq16_wr);
end if;
else -- Wait無し
m_seq16_wr <= (others => '0');
end if;
end if;
end if;
end process;
-- wready の処理、M系列を計算して128以上だったらWaitする。
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
cs_wready <= idle_wready;
wready <= '0';
else
case (cs_wready) is
when idle_wready =>
if wrdat_cs=idle_wrdt and wad_fifo_empty='0' then -- 次はwr_burst
if m_seq16_wr(7)='0' and wres_fifo_full='0' then -- wready='1'
cs_wready <= assert_wready;
wready <= '1';
else -- m_seq16_wr(7)='1' then -- wready='0'
cs_wready <= deassert_wready;
wready <= '0';
end if;
end if;
when assert_wready => -- 一度wreadyがアサートされたら、1つのトランザクションが終了するまでwready='1'
if wrdat_cs=wr_burst and S_AXI_WLAST='1' and S_AXI_WVALID='1' then -- 終了
cs_wready <= idle_wready;
wready <= '0';
elsif wrdat_cs=wr_burst and S_AXI_WVALID='1' then -- 1つのトランザクション終了。
if m_seq16_wr(7)='1' or wres_fifo_full='1' then
cs_wready <= deassert_wready;
wready <= '0';
end if;
end if;
when deassert_wready =>
if m_seq16_wr(7)='0' and wres_fifo_full='0' then -- wready='1'
cs_wready <= assert_wready;
wready <= '1';
end if;
end case;
end if;
end if;
end process;
S_AXI_WREADY <= wready;
cdc_we <= '1' when wrdat_cs=wr_burst and wready='1' and S_AXI_WVALID='1' else '0';
-- addr_inc_step_wr の処理
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
addr_inc_step_wr <= 1;
else
if wrdat_cs=idle_wrdt and wad_fifo_empty='0' then
case (wad_fifo_dout(WAD_FIFO_AWSIZE_HIGH downto WAD_FIFO_AWSIZE_LOW)) is
when "000" => -- 8ビット転送
addr_inc_step_wr <= 1;
when "001" => -- 16ビット転送
addr_inc_step_wr <= 2;
when "010" => -- 32ビット転送
addr_inc_step_wr <= 4;
when "011" => -- 64ビット転送
addr_inc_step_wr <= 8;
when "100" => -- 128ビット転送
addr_inc_step_wr <= 16;
when "101" => -- 256ビット転送
addr_inc_step_wr <= 32;
when "110" => -- 512ビット転送
addr_inc_step_wr <= 64;
when others => --"111" => -- 1024ビット転送
addr_inc_step_wr <= 128;
end case;
end if;
end if;
end if;
end process;
-- wr_addr の処理
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
wr_addr <= (others => '0');
else
if wrdat_cs=idle_wrdt and wad_fifo_empty='0' then
wr_addr <= wad_fifo_dout(C_OFFSET_WIDTH-1 downto 0);
elsif wrdat_cs=wr_burst and S_AXI_WVALID='1' and wready='1' then -- アドレスを進める
wr_addr <= std_logic_vector(unsigned(wr_addr) + addr_inc_step_wr);
end if;
end if;
end if;
end process;
-- Wirte Response FIFO (wres_fifo)
wres_fifo : sync_fifo generic map(
C_MEMORY_SIZE => AD_FIFO_DEPTH,
DATA_BUS_WIDTH => WRES_FIFO_WIDTH
) port map (
clk => ACLK,
rst => reset,
wr_en => wres_fifo_wr_en,
din => wres_fifo_din,
full => wres_fifo_full,
almost_full => wres_fifo_almost_full,
rd_en => wres_fifo_rd_en,
dout => wres_fifo_dout,
empty => wres_fifo_empty,
almost_empty => wres_fifo_almost_empty
);
wres_fifo_wr_en <= '1' when S_AXI_WLAST='1' and S_AXI_WVALID='1' and wready='1' else '0'; -- Write Transaction 終了
wres_fifo_din <= (wad_fifo_dout(WAD_FIFO_AWID_HIGH downto WAD_FIFO_AWID_LOW) & wad_fifo_dout(WAD_FIFO_AWBURST_HIGH downto WAD_FIFO_AWBURST_LOW));
wres_fifo_rd_en <= '1' when wr_bvalid='1' and S_AXI_BREADY='1' and wres_fifo_empty='0' else '0';
-- S_AXI_BID の処理
S_AXI_BID <= wres_fifo_dout(WRES_FIFO_AWID_HIGH downto WRES_FIFO_AWID_LOW);
-- S_AXI_BRESP の処理
-- S_AXI_AWBURSTがINCRの時はOKAYを返す。それ以外はSLVERRを返す。
S_AXI_BRESP <= RESP_OKAY when wres_fifo_dout(WRES_FIFO_AWBURST_HIGH downto WRES_FIFO_AWBURST_LOW)=AxBURST_INCR else RESP_SLVERR;
-- wr_bvalid の処理
-- wr_bvalid のアサートは、Write Data Channelの完了より必ず1クロックは遅延する
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
wrres_cs <= idle_wres;
wr_bvalid <= '0';
else
case( wrres_cs ) is
when idle_wres =>
if wres_fifo_empty='0' then -- Write Transaction 終了
if unsigned(m_seq16_wr_res) = 0 or RANDOM_BVALID_WAIT=0 then
wrres_cs <= bvalid_assert;
wr_bvalid <= '1';
else
wrres_cs <= wait_bvalid;
end if;
end if;
when wait_bvalid =>
if unsigned(wr_resp_cnt) = 0 then
wrres_cs <= bvalid_assert;
wr_bvalid <= '1';
end if;
when bvalid_assert =>
if (S_AXI_BREADY='1') then
wrres_cs <= idle_wres;
wr_bvalid <= '0';
end if;
when others =>
end case ;
end if;
end if;
end process;
S_AXI_BVALID <= wr_bvalid;
S_AXI_BUSER <= (others => '0');
-- wr_resp_cnt
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
wr_resp_cnt <= (others => '0');
else
if wrres_cs=idle_wres and wres_fifo_empty='0' then
wr_resp_cnt <= m_seq16_wr_res(4 downto 0);
elsif unsigned(wr_resp_cnt) /= 0 then
wr_resp_cnt <= std_logic_vector(unsigned(wr_resp_cnt) - 1);
end if;
end if;
end if;
end process;
-- m_seq_wr_res、16ビットのM系列を計算する
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
m_seq16_wr_res <= (0 => '1', others => '0');
else
m_seq16_wr_res <= M_SEQ16_BFM_F(m_seq16_wr_res);
end if;
end if;
end process;
-- AXI4バス Read Address Transaction State Machine
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
rdadr_cs <= idle_rda;
arready <= '0';
else
case (rdadr_cs) is
when idle_rda =>
if S_AXI_ARVALID='1' and rad_fifo_full='0' then -- Read Transaction 要求
rdadr_cs <= arr_accept;
arready <= '1';
end if;
when arr_accept => -- S_AXI_ARREADY をアサート
rdadr_cs <= idle_rda;
arready <= '0';
end case;
end if;
end if;
end process;
S_AXI_ARREADY <= not rad_fifo_full when ARREADY_IS_USUALLY_HIGH=1 else arready;
-- S_AXI_ARID & S_AXI_ARBURST & S_AXI_ARSIZE & S_AXI_ARLEN & S_AXI_ARADDR を保存しておく同期FIFO
rad_fifo_din <= (S_AXI_ARID & S_AXI_ARBURST & S_AXI_ARSIZE & S_AXI_ARLEN & S_AXI_ARADDR);
rad_fifo_wr_en <= (S_AXI_ARVALID and (not rad_fifo_full)) when ARREADY_IS_USUALLY_HIGH=1 else arready;
rad_fifo : sync_fifo generic map (
C_MEMORY_SIZE => AD_FIFO_DEPTH,
DATA_BUS_WIDTH => RAD_FIFO_WIDTH
) port map (
clk => ACLK,
rst => reset,
wr_en => rad_fifo_wr_en,
din => rad_fifo_din,
full => rad_fifo_full,
almost_full => rad_fifo_almost_full,
rd_en => rad_fifo_rd_en,
dout => rad_fifo_dout,
empty => rad_fifo_empty,
almost_empty => rad_fifo_almost_empty
);
rad_fifo_rd_en <= '1' when rvalid='1' and S_AXI_RREADY='1' and rlast='1' else '0';
-- AXI4バス Read Data Transaction State Machine
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
rddat_cs <= idle_rdd;
else
case (rddat_cs) is
when idle_rdd =>
if rad_fifo_empty='0' then -- AXI Read アドレス転送の残りが1個以上ある
rddat_cs <= rd_burst;
end if;
when rd_burst =>
if unsigned(rd_axi_count)=0 and rvalid='1' and S_AXI_RREADY='1' then -- Read Transaction 終了
rddat_cs <= idle_rdd;
end if;
end case;
end if;
end if;
end process;
-- m_seq_rd、16ビットのM系列を計算する
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
m_seq16_rd <= (others => '1'); -- Writeとシードを変更する
else
if READ_RANDOM_WAIT=1 then -- Read Transaciton のデータ転送でランダムなWaitを挿入する場合
if rddat_cs=rd_burst then
m_seq16_rd <= M_SEQ16_BFM_F(m_seq16_rd);
end if;
else -- Wati無し
m_seq16_rd <= (others => '0');
end if;
end if;
end if;
end process;
-- rvalid の処理、M系列を計算して128以上だったらWaitする。
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
cs_rvalid <= idle_rvalid;
rvalid <= '0';
else
case (cs_rvalid) is
when idle_rvalid =>
if rddat_cs=idle_rdd and rad_fifo_empty='0' then -- 次はrd_burst
if m_seq16_rd(7)='0' then -- rvalid='1'
cs_rvalid <= assert_rvalid;
rvalid <= '1';
else -- m_seq16_rd(7)='1' then -- rvalid='0'
cs_rvalid <= deassert_rvalid;
rvalid <= '0';
end if;
end if;
when assert_rvalid => -- 一度rvalidがアサートされたら、1つのトランザクションが終了するまでrvalid='1'
if rddat_cs=rd_burst and rlast='1' and S_AXI_RREADY='1' then -- 終了
cs_rvalid <= idle_rvalid;
rvalid <= '0';
elsif rddat_cs=rd_burst and S_AXI_RREADY='1' then -- 1つのトランザクション終了。
if m_seq16_rd(7)='1' then
cs_rvalid <= deassert_rvalid;
rvalid <= '0';
end if;
end if;
when deassert_rvalid =>
if m_seq16_rd(7)='0' then -- rvalid='1'
cs_rvalid <= assert_rvalid;
rvalid <= '1';
end if;
end case;
end if;
end if;
end process;
S_AXI_RVALID <= rvalid;
-- addr_inc_step_rd の処理
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
addr_inc_step_rd <= 1;
else
if rddat_cs=idle_rdd and rad_fifo_empty='0' then
case (rad_fifo_dout(RAD_FIFO_ARSIZE_HIGH downto RAD_FIFO_ARSIZE_LOW)) is
when "000" => -- 8ビット転送
addr_inc_step_rd <= 1;
when "001" => -- 16ビット転送
addr_inc_step_rd <= 2;
when "010" => -- 32ビット転送
addr_inc_step_rd <= 4;
when "011" => -- 64ビット転送
addr_inc_step_rd <= 8;
when "100" => -- 128ビット転送
addr_inc_step_rd <= 16;
when "101" => -- 256ビット転送
addr_inc_step_rd <= 32;
when "110" => -- 512ビット転送
addr_inc_step_rd <= 64;
when others => -- "111" => -- 1024ビット転送
addr_inc_step_rd <= 128;
end case;
end if;
end if;
end if;
end process;
-- rd_addr の処理
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
rd_addr <= (others => '0');
else
if rddat_cs=idle_rdd and rad_fifo_empty='0' then
rd_addr <= rad_fifo_dout(C_OFFSET_WIDTH-1 downto 0);
elsif rddat_cs=rd_burst and S_AXI_RREADY='1' and rvalid='1' then
rd_addr <= std_logic_vector(unsigned(rd_addr) + addr_inc_step_rd);
end if;
end if;
end if;
end process;
-- rd_axi_count の処理(AXIバス側のデータカウント)
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
rd_axi_count <= (others => '0');
else
if rddat_cs=idle_rdd and rad_fifo_empty='0' then -- rd_axi_count のロード
rd_axi_count <= rad_fifo_dout(RAD_FIFO_ARLEN_HIGH downto RAD_FIFO_ARLEN_LOW);
elsif rddat_cs=rd_burst and rvalid='1' and S_AXI_RREADY='1' then -- Read Transaction が1つ終了
rd_axi_count <= std_logic_vector(unsigned(rd_axi_count) - 1);
end if;
end if;
end if;
end process;
-- rdlast State Machine
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
rdlast <= idle_rlast;
rlast <= '0';
else
case (rdlast) is
when idle_rlast =>
if unsigned(rd_axi_count)=1 and rvalid='1' and S_AXI_RREADY='1' then -- バーストする場合
rdlast <= rlast_assert;
rlast <= '1';
elsif rddat_cs=idle_rdd and rad_fifo_empty='0' and unsigned(rad_fifo_dout(RAD_FIFO_ARLEN_HIGH downto RAD_FIFO_ARLEN_LOW))=0 then -- 転送数が1の場合
rdlast <= rlast_assert;
rlast <= '1';
end if;
when rlast_assert =>
if rvalid='1' and S_AXI_RREADY='1' then -- Read Transaction 終了(rd_axi_count=0は決定)
rdlast <= idle_rlast;
rlast <= '0';
end if;
end case;
end if;
end if;
end process;
S_AXI_RLAST <= rlast;
-- S_AXI_RID, S_AXI_RUSER の処理
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
S_AXI_RID <= (others => '0');
else
if rddat_cs=idle_rdd and rad_fifo_empty='0' then
S_AXI_RID <= rad_fifo_dout(RAD_FIFO_ARID_HIGH downto RAD_FIFO_ARID_LOW);
end if;
end if;
end if;
end process;
S_AXI_RUSER <= (others => '0');
-- S_AXI_RRESP は、S_AXI_ARBURST がINCR の場合はOKAYを返す。それ以外はSLVERRを返す。
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
S_AXI_RRESP <= (others => '0');
else
if rddat_cs=idle_rdd and rad_fifo_empty='0' then
if rad_fifo_dout(RAD_FIFO_ARBURST_HIGH downto RAD_FIFO_ARBURST_LOW)=AxBURST_INCR then
S_AXI_RRESP <= RESP_OKAY;
else
S_AXI_RRESP <= RESP_SLVERR;
end if;
end if;
end if;
end if;
end process;
-- RAM
process (ACLK) begin
if ACLK'event and ACLK='1' then
if cdc_we='1' then
for i in 0 to C_S_AXI_DATA_WIDTH/8-1 loop
if S_AXI_WSTRB(i)='1' then -- Byte Enable
ram_array(TO_INTEGER(unsigned(wr_addr(C_OFFSET_WIDTH-1 downto ADD_INC_OFFSET))))(i*8+7 downto i*8) <= S_AXI_WDATA(i*8+7 downto i*8);
end if;
end loop;
end if;
end if;
end process;
-- Read Transaciton の時に +1 されたReadデータを使用する(Read 毎に+1)
process (ACLK) begin
if ACLK'event and ACLK='1' then
if reset='1' then
read_data_count <= (others => '0');
else
if rddat_cs=rd_burst and rvalid='1' and S_AXI_RREADY='1' then -- Read Transaction が1つ終了
read_data_count <= std_logic_vector(unsigned(read_data_count) + 1);
end if;
end if;
end if;
end process;
S_AXI_RDATA <= ram_array(TO_INTEGER(unsigned(rd_addr(C_OFFSET_WIDTH-1 downto ADD_INC_OFFSET)))) when READ_DATA_IS_INCREMENT=0 else read_data_count;
end implementation;
-- Synchronous FIFO for Simulation
--
-- 2014/07/03 by marsee
--
-- ライセンスは二条項BSDライセンス (2-clause BSD license)とします。
--
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.std_logic_misc.all;
use IEEE.math_real.all;
entity sync_fifo is
generic (
C_MEMORY_SIZE : integer := 512; -- Word (not byte), 2のn乗
DATA_BUS_WIDTH : integer := 32 -- RAM Data Width
);
port (
clk : in std_logic;
rst : in std_logic;
wr_en : in std_logic;
din : in std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
full : out std_logic;
almost_full : out std_logic;
rd_en : in std_logic;
dout : out std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
empty : out std_logic;
almost_empty : out std_logic
);
end sync_fifo;
architecture RTL of sync_fifo is
constant C_MEMORY_LENGTH : natural := natural(ceil(log(real(C_MEMORY_SIZE), 2.0))); -- C_MEMORY_SIZEの2進数の桁数
type mem_type is array (0 to C_MEMORY_SIZE-1) of std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
signal mem : mem_type := (OTHERS => (OTHERS => '0'));
signal mem_waddr : std_logic_vector(C_MEMORY_LENGTH-1 downto 0) := (others => '0');
signal mem_raddr : std_logic_vector(C_MEMORY_LENGTH-1 downto 0) := (others => '0');
signal rp : std_logic_vector(C_MEMORY_LENGTH-1 downto 0) := (others => '0');
signal wp : std_logic_vector(C_MEMORY_LENGTH-1 downto 0) := (others => '0');
signal almost_full_node : std_logic;
signal almost_empty_node : std_logic;
signal full_node : std_logic;
signal empty_node : std_logic;
begin
-- Write
process (clk) begin
if clk'event and clk='1' then
if rst='1' then
mem_waddr <= (others => '0');
wp <= (others => '0');
else
if wr_en='1' then
mem_waddr <= std_logic_vector(unsigned(mem_waddr) + 1);
wp <= std_logic_vector(unsigned(wp) + 1);
end if;
end if;
end if;
end process;
process (clk) begin
if clk'event and clk='1' then
if wr_en='1' then
mem(TO_INTEGER(unsigned(mem_waddr))) <= din;
end if;
end if;
end process;
full_node <= '1' when std_logic_vector(unsigned(wp)+1)=rp else '0';
full <= full_node;
almost_full_node <= '1' when std_logic_vector(unsigned(wp)+2)=rp else '0';
almost_full <= full_node or almost_full_node;
-- Read
process (clk) begin
if clk'event and clk='1' then
if rst='1' then
mem_raddr <= (others => '0');
rp <= (others => '0');
else
if rd_en='1' then
mem_raddr <= std_logic_vector(unsigned(mem_raddr) + 1);
rp <= std_logic_vector(unsigned(rp) + 1);
end if;
end if;
end if;
end process;
dout <= mem(TO_INTEGER(unsigned(mem_raddr)));
empty_node <= '1' when wp=rp else '0';
empty <= empty_node;
almost_empty_node <= '1' when wp=std_logic_vector(unsigned(rp)+1) else '0';
almost_empty <= empty_node or almost_empty_node;
end RTL;
/* AXI Master用 Slave Bus Function Mode (BFM)
axi_slave_BFM.v
2012/02/25 : S_AXI_AWBURST=1 (INCR) にのみ対応、AWSIZE, ARSIZE = 000 (1byte), 001 (2bytes), 010 (4bytes) のみ対応。
2012/07/04 : READ_ONLY_TRANSACTION を追加。Read機能のみでも+1したデータを出力することが出来るように変更した。
2014/01/05 : ポート名をM_AXI?からS_AXI?に修正、Verilogに移植(By Koba)
*/
// 2014/07/18 : Write Respose Channel に sync_fifo を使用した by marsee
// 2014/08/31 : READ_RANDOM_WAIT=1 の時に、S_AXI_RREADY が S_AXI_RVALID に依存するバグをフィック。 by marsee
// WRITE_RANDOM_WAIT=1 の時に、S_AXI_WVALID が S_AXI_WREADY に依存するバグをフィック。 by marsee
//
// 2016/07/03 : AWREADY_IS_USUALLY_HIGH と ARREADY_IS_USUALLY_HIGH の2つのパラメータを追加 by marsee
//
// ライセンスは二条項BSDライセンス (2-clause BSD license)とします。
//
module axi_slave_bfm #(
parameter integer C_S_AXI_ID_WIDTH = 1,
parameter integer C_S_AXI_ADDR_WIDTH = 32,
parameter integer C_S_AXI_DATA_WIDTH = 32,
parameter integer C_S_AXI_AWUSER_WIDTH = 1,
parameter integer C_S_AXI_ARUSER_WIDTH = 1,
parameter integer C_S_AXI_WUSER_WIDTH = 1,
parameter integer C_S_AXI_RUSER_WIDTH = 1,
parameter integer C_S_AXI_BUSER_WIDTH = 1,
parameter integer C_S_AXI_TARGET = 0,
parameter integer C_OFFSET_WIDTH = 10, // 割り当てるRAMのアドレスのビット幅
parameter integer C_S_AXI_BURST_LEN = 256,
parameter integer WRITE_RANDOM_WAIT = 1, // Write Transactionデータ転送時にランダムなWaitを発生させる=1、Waitしない=0
parameter integer READ_RANDOM_WAIT = 0, // Read Transactionデータ転送時にランダムなWaitを発生させる=1、Waitしない=0
parameter integer READ_DATA_IS_INCREMENT = 0, // Read TransactionでRAMのデータを読み出す=0、0はじまりの+1データを使う=1
parameter integer RANDOM_BVALID_WAIT = 0, // Write Transaction後、BVALIDをランダムにWaitする=1、ランダムにWaitしない=0
parameter integer AWREADY_IS_USUALLY_HIGH = 1, // AWRAEDY は通常はLow=0, High=1
parameter integer ARREADY_IS_USUALLY_HIGH = 1 // AWRAEDY は通常はLow=0, High=1
)
(
// System Signals
input ACLK,
input ARESETN,
// Slave Interface Write Address Ports
input [C_S_AXI_ID_WIDTH-1 : 0] S_AXI_AWID,
input [C_S_AXI_ADDR_WIDTH-1 : 0] S_AXI_AWADDR,
input [8-1 : 0] S_AXI_AWLEN,
input [3-1 : 0] S_AXI_AWSIZE,
input [2-1 : 0] S_AXI_AWBURST,
// input S_AXI_AWLOCK [2-1 : 0],
input [1 : 0] S_AXI_AWLOCK,
input [4-1 : 0] S_AXI_AWCACHE,
input [3-1 : 0] S_AXI_AWPROT,
input [4-1 : 0] S_AXI_AWQOS,
input [C_S_AXI_AWUSER_WIDTH-1 :0] S_AXI_AWUSER,
input S_AXI_AWVALID,
output S_AXI_AWREADY,
// Slave Interface Write Data Ports
input [C_S_AXI_DATA_WIDTH-1 : 0] S_AXI_WDATA,
input [C_S_AXI_DATA_WIDTH/8-1 : 0]S_AXI_WSTRB,
input S_AXI_WLAST,
input [C_S_AXI_WUSER_WIDTH-1 : 0] S_AXI_WUSER,
input S_AXI_WVALID,
output S_AXI_WREADY,
// Slave Interface Write Response Ports
output [C_S_AXI_ID_WIDTH-1 : 0] S_AXI_BID,
output [2-1 : 0] S_AXI_BRESP,
output [C_S_AXI_BUSER_WIDTH-1 : 0] S_AXI_BUSER,
output S_AXI_BVALID,
input S_AXI_BREADY,
// Slave Interface Read Address Ports
input [C_S_AXI_ID_WIDTH-1 : 0] S_AXI_ARID,
input [C_S_AXI_ADDR_WIDTH-1 : 0] S_AXI_ARADDR,
input [8-1 : 0] S_AXI_ARLEN,
input [3-1 : 0] S_AXI_ARSIZE,
input [2-1 : 0] S_AXI_ARBURST,
input [2-1 : 0] S_AXI_ARLOCK,
input [4-1 : 0] S_AXI_ARCACHE,
input [3-1 : 0] S_AXI_ARPROT,
input [4-1 : 0] S_AXI_ARQOS,
input [C_S_AXI_ARUSER_WIDTH-1 : 0]S_AXI_ARUSER,
input S_AXI_ARVALID,
output S_AXI_ARREADY,
// Slave Interface Read Data Ports
output reg [C_S_AXI_ID_WIDTH-1: 0] S_AXI_RID,
output [C_S_AXI_DATA_WIDTH-1 : 0] S_AXI_RDATA,
output reg [2-1 : 0] S_AXI_RRESP,
output S_AXI_RLAST,
output [C_S_AXI_RUSER_WIDTH-1 : 0] S_AXI_RUSER,
output S_AXI_RVALID,
input S_AXI_RREADY
);
localparam AXBURST_FIXED = 2'b00;
localparam AXBURST_INCR = 2'b01;
localparam AXBURST_WRAP = 2'b10;
localparam RESP_OKAY = 2'b00;
localparam RESP_EXOKAY = 2'b01;
localparam RESP_SLVERR = 2'b10;
localparam RESP_DECERR = 2'b11;
localparam DATA_BUS_BYTES = (C_S_AXI_DATA_WIDTH / 8);
//localparam ADD_INC_OFFSET = log2(DATA_BUS_BYTES);
localparam ADD_INC_OFFSET = (DATA_BUS_BYTES==1) ? 0:
(DATA_BUS_BYTES==2) ? 1:
(DATA_BUS_BYTES==4) ? 2:
(DATA_BUS_BYTES==8) ? 3:
(DATA_BUS_BYTES==16) ? 4:
(DATA_BUS_BYTES==32) ? 5:
(DATA_BUS_BYTES==64) ? 6:
(DATA_BUS_BYTES==128) ? 7: 32'hxxxxxxxx;
// fifo depth for address
localparam AD_FIFO_DEPTH = 16;
// wad_fifo field
localparam WAD_FIFO_WIDTH = C_S_AXI_ADDR_WIDTH+5+C_S_AXI_ID_WIDTH-1+1;
localparam WAD_FIFO_AWID_HIGH = C_S_AXI_ADDR_WIDTH+5+C_S_AXI_ID_WIDTH-1;
localparam WAD_FIFO_AWID_LOW = C_S_AXI_ADDR_WIDTH+5;
localparam WAD_FIFO_AWBURST_HIGH = C_S_AXI_ADDR_WIDTH+4;
localparam WAD_FIFO_AWBURST_LOW = C_S_AXI_ADDR_WIDTH+3;
localparam WAD_FIFO_AWSIZE_HIGH = C_S_AXI_ADDR_WIDTH+2;
localparam WAD_FIFO_AWSIZE_LOW = C_S_AXI_ADDR_WIDTH;
localparam WAD_FIFO_ADDR_HIGH = C_S_AXI_ADDR_WIDTH-1;
localparam WAD_FIFO_ADDR_LOW = 0;
// wres_fifo field
localparam WRES_FIFO_WIDTH = 2+C_S_AXI_ID_WIDTH-1+1;
localparam WRES_FIFO_AWID_HIGH = 2+C_S_AXI_ID_WIDTH-1;
localparam WRES_FIFO_AWID_LOW = 2;
localparam WRES_FIFO_AWBURST_HIGH = 1;
localparam WRES_FIFO_AWBURST_LOW = 0;
// rad_fifo field
localparam RAD_FIFO_WIDTH = C_S_AXI_ADDR_WIDTH+13+C_S_AXI_ID_WIDTH-1+1;
localparam RAD_FIFO_ARID_HIGH = C_S_AXI_ADDR_WIDTH+13+C_S_AXI_ID_WIDTH-1;
localparam RAD_FIFO_ARID_LOW = C_S_AXI_ADDR_WIDTH+13;
localparam RAD_FIFO_ARBURST_HIGH = C_S_AXI_ADDR_WIDTH+12;
localparam RAD_FIFO_ARBURST_LOW = C_S_AXI_ADDR_WIDTH+11;
localparam RAD_FIFO_ARSIZE_HIGH = C_S_AXI_ADDR_WIDTH+10;
localparam RAD_FIFO_ARSIZE_LOW = C_S_AXI_ADDR_WIDTH+8;
localparam RAD_FIFO_ARLEN_HIGH = C_S_AXI_ADDR_WIDTH+7;
localparam RAD_FIFO_ARLEN_LOW = C_S_AXI_ADDR_WIDTH;
localparam RAD_FIFO_ADDR_HIGH = C_S_AXI_ADDR_WIDTH-1;
localparam RAD_FIFO_ADDR_LOW = 0;
// RAMの生成
localparam SLAVE_ADDR_NUMBER = 2 ** (C_OFFSET_WIDTH - ADD_INC_OFFSET);
reg [(C_S_AXI_DATA_WIDTH - 1):0] ram_array [(SLAVE_ADDR_NUMBER - 1):0];
// for write transaction
// write_address_state
localparam IDLE_WRAD = 1'd0;
localparam AWR_ACCEPT = 1'd1;
reg wradr_cs;
// write_data_state
localparam IDLE_WRDT = 1'd0;
localparam WR_BURST = 1'd1;
reg wrdat_cs;
// write_response_state
localparam IDLE_WRES = 2'd0;
localparam WAIT_BVALID = 2'd1;
localparam BVALID_ASSERT = 2'd2;
reg [1:0] wrres_cs;
integer addr_inc_step_wr = 1;
reg awready;
reg [(C_OFFSET_WIDTH - 1):0] wr_addr;
reg [(C_S_AXI_ID_WIDTH - 1):0] wr_bid;
reg [1:0] wr_bresp;
reg wr_bvalid;
reg [15:0] m_seq16_wr;
reg wready;
// wready_state
localparam IDLE_WREADY = 2'd0;
localparam ASSERT_WREADY = 2'd1;
localparam DEASSERT_WREADY = 2'd2;
reg [1:0] cs_wready;
wire cdc_we;
wire wad_fifo_full;
wire wad_fifo_empty;
wire wad_fifo_almost_full;
wire wad_fifo_almost_empty;
wire wad_fifo_rd_en;
wire wad_fifo_wr_en;
wire [WAD_FIFO_WIDTH-1:0] wad_fifo_din;
wire [WAD_FIFO_WIDTH-1:0] wad_fifo_dout;
reg [15:0] m_seq16_wr_res;
reg [4:0] wr_resp_cnt;
// wres_fifo
wire wres_fifo_wr_en;
wire wres_fifo_full;
wire wres_fifo_empty;
wire wres_fifo_almost_full;
wire wres_fifo_almost_empty;
wire wres_fifo_rd_en;
wire [WRES_FIFO_WIDTH-1:0] wres_fifo_din;
wire [WRES_FIFO_WIDTH-1:0] wres_fifo_dout;
// for read transaction
// read_address_state
localparam IDLE_RDA = 1'd0;
localparam ARR_ACCEPT = 1'd1;
reg rdadr_cs;
// read_data_state
localparam IDLE_RDD = 1'd0;
localparam RD_BURST = 1'd1;
reg rddat_cs;
// read_last_state
localparam IDLE_RLAST = 1'd0;
localparam RLAST_ASSERT = 1'd1;
reg rdlast;
integer addr_inc_step_rd = 1;
reg arready;
reg [(C_OFFSET_WIDTH - 1):0] rd_addr;
reg [7:0] rd_axi_count;
reg rvalid;
reg rlast;
reg [15:0] m_seq16_rd;
// rvalid_state
localparam IDLE_RVALID = 2'd0;
localparam ASSERT_RVALID = 2'd1;
localparam DEASSERT_RVALID = 2'd2;
reg [1:0] cs_rvalid;
reg [(C_S_AXI_DATA_WIDTH - 1):0] read_data_count;
reg reset_1d;
reg reset_2d;
wire reset;
wire rad_fifo_full;
wire rad_fifo_empty;
wire rad_fifo_almost_full;
wire rad_fifo_almost_empty;
wire rad_fifo_wr_en;
wire rad_fifo_rd_en;
wire [RAD_FIFO_WIDTH-1:0] rad_fifo_din;
wire [RAD_FIFO_WIDTH-1:0] rad_fifo_dout;
// ARESETN をACLK で同期化
always @ ( posedge ACLK ) begin
reset_1d <= ~ARESETN;
reset_2d <= reset_1d;
end
assign reset = reset_2d;
// AXI4バス Write Address State Machine
always @ ( posedge ACLK ) begin
if ( reset ) begin
wradr_cs <= IDLE_WRAD;
awready <= 1'b0;
end
else
case (wradr_cs)
IDLE_WRAD: if ((S_AXI_AWVALID == 1'b1) && (wad_fifo_full == 1'b0) && (wres_fifo_full == 1'b0)) // S_AXI_AWVALIDが1にアサートされた
begin
wradr_cs <= AWR_ACCEPT;
awready <= 1'b1;
end
AWR_ACCEPT: begin
wradr_cs <= IDLE_WRAD;
awready <= 1'b0;
end
endcase
end
assign S_AXI_AWREADY = (AWREADY_IS_USUALLY_HIGH==1) ? ~wad_fifo_full : awready;
// {S_AXI_AWID, S_AXI_AWBURST, S_AXI_AWSIZE, S_AXI_AWADDR}を保存しておく同期FIFO
assign wad_fifo_din = {S_AXI_AWID, S_AXI_AWBURST, S_AXI_AWSIZE, S_AXI_AWADDR};
assign wad_fifo_wr_en = (AWREADY_IS_USUALLY_HIGH==1) ? (S_AXI_AWVALID & ~wad_fifo_full) : awready;
sync_fifo #(
.C_MEMORY_SIZE (AD_FIFO_DEPTH),
.DATA_BUS_WIDTH (WAD_FIFO_WIDTH)
) wad_fifo (
.clk (ACLK),
.rst (reset),
.wr_en (wad_fifo_wr_en),
.din (wad_fifo_din),
.full (wad_fifo_full),
.almost_full (wad_fifo_almost_full),
.rd_en (wad_fifo_rd_en),
.dout (wad_fifo_dout),
.empty (wad_fifo_empty),
.almost_empty (wad_fifo_almost_empty)
);
assign wad_fifo_rd_en = (wready & S_AXI_WVALID & S_AXI_WLAST);
// AXI4バス Write Data State Machine
always @( posedge ACLK ) begin
if ( reset )
wrdat_cs <= IDLE_WRDT;
else
case (wrdat_cs)
IDLE_WRDT: if ( wad_fifo_empty == 1'b0 ) // AXI Writeアドレス転送の残りが1個以上ある
wrdat_cs <= WR_BURST;
WR_BURST : if ( S_AXI_WLAST & S_AXI_WVALID & wready ) // Write Transaction終了
wrdat_cs <= IDLE_WRDT;
endcase
end
// M系列による16ビット乱数生成関数
function [15:0] M_SEQ16_BFM_F;
input [15:0] mseq16in;
reg xor_result;
begin
xor_result = mseq16in[15] ^ mseq16in[12] ^ mseq16in[10] ^ mseq16in[8] ^
mseq16in[7] ^ mseq16in[6] ^ mseq16in[3] ^ mseq16in[2];
M_SEQ16_BFM_F = {mseq16in[14:0], xor_result};
end
endfunction
// m_seq_wr、16ビットのM系列を計算する
always @( posedge ACLK ) begin
if ( reset )
m_seq16_wr <= 16'b1;
else begin
if ( WRITE_RANDOM_WAIT ) begin // Write Transaction時にランダムなWaitを挿入する
if ( wrdat_cs == WR_BURST )
m_seq16_wr <= M_SEQ16_BFM_F(m_seq16_wr);
end
else // Wait無し
m_seq16_wr <= 16'b0;
end
end
// wready の処理、M系列を計算して128以上だったらWaitする。
always @( posedge ACLK ) begin
if ( reset ) begin
cs_wready <= IDLE_WREADY;
wready <= 1'b0;
end
else
case (cs_wready)
IDLE_WREADY: if ( (wrdat_cs == IDLE_WRDT) && (wad_fifo_empty == 1'b0) ) begin
if ( (m_seq16_wr[7] == 1'b0) && (wres_fifo_full==1'b0) ) begin
cs_wready <= ASSERT_WREADY;
wready <= 1'b1;
end
else begin
cs_wready <= DEASSERT_WREADY;
wready <= 1'b0;
end
end
ASSERT_WREADY: if ( (wrdat_cs == WR_BURST) && S_AXI_WLAST && S_AXI_WVALID ) begin
cs_wready <= IDLE_WREADY;
wready <= 1'b0;
end
else if ( (wrdat_cs == WR_BURST) && S_AXI_WVALID ) begin
if ((m_seq16_wr[7] == 1'b1) || (wres_fifo_full==1'b1)) begin
cs_wready <= DEASSERT_WREADY;
wready <= 1'b0;
end
end
DEASSERT_WREADY:if ( (m_seq16_wr[7] == 1'b0) && (wres_fifo_full==1'b0) ) begin
cs_wready <= ASSERT_WREADY;
wready <= 1'b1;
end
endcase
end
assign S_AXI_WREADY = wready;
assign cdc_we = ( (wrdat_cs == WR_BURST) && wready && S_AXI_WVALID );
// addr_inc_step_wrの処理
always @ ( posedge ACLK ) begin
if ( reset )
addr_inc_step_wr <= 1;
else begin
if ( (wrdat_cs == IDLE_WRDT) & (wad_fifo_empty == 1'b0) )
case (wad_fifo_dout[WAD_FIFO_AWSIZE_HIGH:WAD_FIFO_AWSIZE_LOW])
3'b000 : addr_inc_step_wr <= 1; // 8ビット転送
3'b001 : addr_inc_step_wr <= 2; // 16ビット転送
3'b010 : addr_inc_step_wr <= 4; // 32ビット転送
3'b011 : addr_inc_step_wr <= 8; // 64ビット転送
3'b100 : addr_inc_step_wr <= 16; // 128ビット転送
3'b101 : addr_inc_step_wr <= 32; // 256ビット転送
3'b110 : addr_inc_step_wr <= 64; // 512ビット転送
default: addr_inc_step_wr <= 128; // 1024ビット転送
endcase
end
end
// wr_addr の処理
always @ (posedge ACLK ) begin
if ( reset )
wr_addr <= 'b0;
else begin
if ( (wrdat_cs == IDLE_WRDT) && (wad_fifo_empty == 1'b0) )
wr_addr <= wad_fifo_dout[(C_OFFSET_WIDTH - 1):0];
else if ( (wrdat_cs == WR_BURST) && S_AXI_WVALID && wready ) // アドレスを進める
wr_addr <= (wr_addr + addr_inc_step_wr);
end
end
// Wirte Response FIFO (wres_fifo)
sync_fifo #(
.C_MEMORY_SIZE(AD_FIFO_DEPTH),
.DATA_BUS_WIDTH(WRES_FIFO_WIDTH)
) wres_fifo (
.clk(ACLK),
.rst(reset),
.wr_en(wres_fifo_wr_en),
.din(wres_fifo_din),
.full(wres_fifo_full),
.almost_full(wres_fifo_almost_full),
.rd_en(wres_fifo_rd_en),
.dout(wres_fifo_dout),
.empty(wres_fifo_empty),
.almost_empty(wres_fifo_almost_empty)
);
assign wres_fifo_wr_en = (S_AXI_WLAST & S_AXI_WVALID & wready) ? 1'b1 : 1'b0; // Write Transaction 終了
assign wres_fifo_din = {wad_fifo_dout[WAD_FIFO_AWID_HIGH:WAD_FIFO_AWID_LOW], wad_fifo_dout[WAD_FIFO_AWBURST_HIGH:WAD_FIFO_AWBURST_LOW]};
assign wres_fifo_rd_en = (wr_bvalid & S_AXI_BREADY) ? 1'b1 : 1'b0;
// S_AXI_BID の処理
assign S_AXI_BID = wres_fifo_dout[WRES_FIFO_AWID_HIGH:WRES_FIFO_AWID_LOW];
// S_AXI_BRESP の処理
// S_AXI_AWBURSTがINCRの時はOKAYを返す。それ以外はSLVERRを返す。
assign S_AXI_BRESP = (wres_fifo_dout[WRES_FIFO_AWBURST_HIGH:WRES_FIFO_AWBURST_LOW]==AXBURST_INCR) ? RESP_OKAY : RESP_SLVERR;
// wr_bvalid の処理
// wr_bvalid のアサートは、Write Data Channelの完了より必ず1クロックは遅延する
always @ ( posedge ACLK ) begin
if ( reset ) begin
wrres_cs <= IDLE_WRES;
wr_bvalid <= 1'b0;
end
else
case (wrres_cs)
IDLE_WRES: if ( wres_fifo_empty == 1'b0 ) begin // Write Transaction 終了
if ( (m_seq16_wr_res == 0) || (RANDOM_BVALID_WAIT == 0) ) begin
wrres_cs <= BVALID_ASSERT;
wr_bvalid <= 1'b1;
end
else
wrres_cs <= WAIT_BVALID;
end
WAIT_BVALID:if ( wr_resp_cnt == 0 ) begin
wrres_cs <= BVALID_ASSERT;
wr_bvalid <= 1'b1;
end
BVALID_ASSERT: if ( S_AXI_BREADY ) begin
wrres_cs <= IDLE_WRES;
wr_bvalid <= 1'b0;
end
endcase
end
assign S_AXI_BVALID = wr_bvalid;
assign S_AXI_BUSER = 'b0;
// wr_resp_cnt
always @ ( posedge ACLK ) begin
if ( reset )
wr_resp_cnt <= 'b0;
else begin
if ( (wrres_cs == IDLE_WRES) && (wres_fifo_empty==1'b0) )
wr_resp_cnt <= m_seq16_wr_res[4:0];
else if ( wr_resp_cnt!=0 )
wr_resp_cnt <= wr_resp_cnt - 1;
end
end
// m_seq_wr_res、16ビットのM系列を計算する
always @ ( posedge ACLK ) begin
if ( reset )
m_seq16_wr_res <= 16'b1;
else
m_seq16_wr_res <= M_SEQ16_BFM_F(m_seq16_wr_res);
end
// AXI4バス Read Address Transaction State Machine
always @ ( posedge ACLK ) begin
if ( reset ) begin
rdadr_cs <= IDLE_RDA;
arready <= 1'b0;
end
else
case (rdadr_cs)
IDLE_RDA: if ( (S_AXI_ARVALID == 1'b1) && (rad_fifo_full == 1'b0) ) begin // Read Transaction要求
rdadr_cs <= ARR_ACCEPT;
arready <= 1'b1;
end
ARR_ACCEPT: begin // S_AXI_ARREADYをアサート
rdadr_cs <= IDLE_RDA;
arready <= 1'b0;
end
endcase
end
assign S_AXI_ARREADY = (ARREADY_IS_USUALLY_HIGH==1) ? ~rad_fifo_full : arready;
// S_AXI_ARID & S_AXI_ARBURST & S_AXI_ARSIZE & S_AXI_ARLEN & S_AXI_ARADDR を保存しておく同期FIFO
assign rad_fifo_din ={S_AXI_ARID, S_AXI_ARBURST, S_AXI_ARSIZE, S_AXI_ARLEN, S_AXI_ARADDR};
assign rad_fifo_wr_en = (ARREADY_IS_USUALLY_HIGH==1) ? (S_AXI_ARVALID & ~rad_fifo_full) : arready;
sync_fifo #(
.C_MEMORY_SIZE (AD_FIFO_DEPTH),
.DATA_BUS_WIDTH (RAD_FIFO_WIDTH)
) rad_fifo
(
.clk (ACLK),
.rst (reset),
.wr_en (rad_fifo_wr_en),
.din (rad_fifo_din),
.full (rad_fifo_full),
.almost_full (rad_fifo_almost_full),
.rd_en (rad_fifo_rd_en),
.dout (rad_fifo_dout),
.empty (rad_fifo_empty),
.almost_empty (rad_fifo_almost_empty)
);
assign rad_fifo_rd_en = (rvalid & S_AXI_RREADY & rlast);
// AXI4バス Read Data Transaction State Machine
always @( posedge ACLK ) begin
if ( reset )
rddat_cs <= IDLE_RDD;
else
case (rddat_cs)
IDLE_RDD: if ( rad_fifo_empty == 1'b0 ) // AXI Read アドレス転送の残りが1個以上ある
rddat_cs <= RD_BURST;
RD_BURST: if ( (rd_axi_count == 0) && rvalid && S_AXI_RREADY ) // Read Transaction終了
rddat_cs <= IDLE_RDD;
endcase
end
// m_seq_rd、16ビットのM系列を計算する
always @ ( posedge ACLK ) begin
if ( reset )
m_seq16_rd <= 16'hffff;
else begin
if ( READ_RANDOM_WAIT) begin
if ( rddat_cs == RD_BURST )
m_seq16_rd <= M_SEQ16_BFM_F(m_seq16_rd);
end else
m_seq16_rd <= 16'b0;
end
end
// rvalidの処理、M系列を計算して128以上だったらWaitする
always @( posedge ACLK ) begin
if ( reset ) begin
cs_rvalid <= IDLE_RVALID;
rvalid <= 1'b0;
end
else
case (cs_rvalid)
IDLE_RVALID: if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) ) begin // 次はrd_burst
if ( m_seq16_rd[7] == 1'b0 ) begin
cs_rvalid <= ASSERT_RVALID;
rvalid <= 1'b1;
end
else begin
cs_rvalid <= DEASSERT_RVALID;
rvalid <= 1'b0;
end
end
ASSERT_RVALID: if ( (rddat_cs == RD_BURST) && rlast && S_AXI_RREADY ) begin // 終了
cs_rvalid <= IDLE_RVALID;
rvalid <= 1'b0;
end
else if ( (rddat_cs == RD_BURST) & S_AXI_RREADY ) begin // 1つのトランザクション終了
if ( m_seq16_rd[7] ) begin
cs_rvalid <= DEASSERT_RVALID;
rvalid <= 1'b0;
end
end
DEASSERT_RVALID:if ( m_seq16_rd[7] == 1'b0 ) begin
cs_rvalid <= ASSERT_RVALID;
rvalid <= 1'b1;
end
endcase
end
assign S_AXI_RVALID = rvalid;
// addr_inc_step_rdの処理
always @( posedge ACLK ) begin
if ( reset )
addr_inc_step_rd <= 1;
else begin
if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) )
case (rad_fifo_dout[RAD_FIFO_ARSIZE_HIGH:RAD_FIFO_ARSIZE_LOW])
3'b000: addr_inc_step_rd <= 1; // 8ビット転送
3'b001: addr_inc_step_rd <= 2; // 16ビット転送
3'b010: addr_inc_step_rd <= 4; // 32ビット転送
3'b011: addr_inc_step_rd <= 8; // 64ビット転送
3'b100: addr_inc_step_rd <= 16; // 128ビット転送
3'b101: addr_inc_step_rd <= 32; // 256ビット転送
3'b110: addr_inc_step_rd <= 64; // 512ビット転送
default:addr_inc_step_rd <= 128; // 1024ビット転送
endcase
end
end
// rd_addr の処理
always @ ( posedge ACLK ) begin
if ( reset )
rd_addr <= 'b0;
else begin
if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) )
rd_addr <= rad_fifo_dout[(C_OFFSET_WIDTH - 1):0];
else if ( (rddat_cs == RD_BURST) && S_AXI_RREADY && rvalid )
rd_addr <= (rd_addr + addr_inc_step_rd);
end
end
// rd_axi_countの処理(AXIバス側のデータカウント)
always @ ( posedge ACLK ) begin
if ( reset )
rd_axi_count <= 'b0;
else begin
if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) ) // rd_axi_countのロード
rd_axi_count <= rad_fifo_dout[RAD_FIFO_ARLEN_HIGH:RAD_FIFO_ARLEN_LOW];
else if ( (rddat_cs == RD_BURST) && rvalid && S_AXI_RREADY ) // Read Transactionが1つ終了
rd_axi_count <= rd_axi_count - 1;
end
end
// rdlast State Machine
always @ ( posedge ACLK ) begin
if ( reset ) begin
rdlast <= IDLE_RLAST;
rlast <= 1'b0;
end
else
case (rdlast)
IDLE_RLAST: if ( (rd_axi_count == 1) && rvalid && S_AXI_RREADY ) begin // バーストする場合
rdlast <= RLAST_ASSERT;
rlast <= 1'b1;
end
else if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) &&
(rad_fifo_dout[RAD_FIFO_ARLEN_HIGH:RAD_FIFO_ARLEN_LOW] == 0) ) begin // 転送数が1の場合
rdlast <= RLAST_ASSERT;
rlast <= 1'b1;
end
RLAST_ASSERT:if ( rvalid && S_AXI_RREADY ) begin // Read Transaction終了(rd_axi_count=0は決定)
rdlast <= IDLE_RLAST;
rlast <= 1'b0;
end
endcase
end
assign S_AXI_RLAST = rlast;
// S_AXI_RID, S_AXI_RUSER の処理
always @ ( posedge ACLK ) begin
if ( reset )
S_AXI_RID <= 'b0;
else begin
if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) )
S_AXI_RID <= rad_fifo_dout[RAD_FIFO_ARID_HIGH:RAD_FIFO_ARID_LOW];
end
end
assign S_AXI_RUSER = 'b0;
// S_AXI_RRESP は、S_AXI_ARBURST がINCR の場合はOKAYを返す。それ以外はSLVERRを返す。
always @( posedge ACLK ) begin
if ( reset )
S_AXI_RRESP <= 'b0;
else begin
if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) ) begin
if ((rad_fifo_dout[RAD_FIFO_ARBURST_HIGH:RAD_FIFO_ARBURST_LOW] == AXBURST_INCR))
S_AXI_RRESP <= RESP_OKAY;
else
S_AXI_RRESP <= RESP_SLVERR;
end
end
end
// RAM
integer i;
always @( posedge ACLK ) begin
if ( cdc_we ) begin :Block_Name_2
for (i=0; i<(C_S_AXI_DATA_WIDTH / 8); i=i+1) begin
if ( S_AXI_WSTRB[i] )
ram_array[wr_addr[(C_OFFSET_WIDTH - 1):ADD_INC_OFFSET]][(i * 8) +: 8]
<= S_AXI_WDATA[(i * 8) +: 8];
end
end
end
// Read Transaciton の時に +1 されたReadデータを使用する(Read 毎に+1)
always @( posedge ACLK ) begin
if ( reset )
read_data_count <= 'b0;
else begin
if ( (rddat_cs == RD_BURST) && rvalid && S_AXI_RREADY )
read_data_count <= read_data_count + 1;
end
end
assign S_AXI_RDATA = (READ_DATA_IS_INCREMENT == 0) ?
ram_array[rd_addr[(C_OFFSET_WIDTH - 1):ADD_INC_OFFSET]] : read_data_count;
endmodule
// Synchronous FIFO for Simulation
//
// 2013/11/08 by marsee
//
// ライセンスは二条項BSDライセンス (2-clause BSD license)とします。
//
`default_nettype none
module sync_fifo #(
parameter integer C_MEMORY_SIZE = 512, // Word (not byte), 2のn乗
parameter integer DATA_BUS_WIDTH = 32 // RAM Data Width
)
(
input wire clk,
input wire rst,
input wire wr_en,
input wire [DATA_BUS_WIDTH-1:0] din,
output wire full,
output wire almost_full,
input wire rd_en,
output wire [DATA_BUS_WIDTH-1:0] dout,
output wire empty,
output wire almost_empty
);
// Beyond Circuts, Constant Function in Verilog 2001を参照しました
// http://www.beyond-circuits.com/wordpress/2008/11/constant-functions/
function integer log2;
input integer addr;
begin
addr = addr - 1;
for (log2=0; addr>0; log2=log2+1)
addr = addr >> 1;
end
endfunction
reg [DATA_BUS_WIDTH-1:0] mem [0:C_MEMORY_SIZE-1];
reg [log2(C_MEMORY_SIZE)-1:0] mem_waddr = 0;
reg [log2(C_MEMORY_SIZE)-1:0] mem_raddr = 0;
reg [log2(C_MEMORY_SIZE)-1:0] rp = 0;
reg [log2(C_MEMORY_SIZE)-1:0] wp = 0;
wire [log2(C_MEMORY_SIZE)-1:0] plus_1 = 1;
wire [log2(C_MEMORY_SIZE)-1:0] plus_2 = 2;
wire almost_full_node;
wire almost_empty_node;
integer i;
// initialize RAM Data
initial begin
for (i=0; i<C_MEMORY_SIZE; i=i+1)
mem[i] = 0;
end
// Write
always @(posedge clk) begin
if (rst) begin
mem_waddr <= 0;
wp <= 0;
end else begin
if (wr_en) begin
mem_waddr <= mem_waddr + 1;
wp <= wp + 1;
end
end
end
always @(posedge clk) begin
if (wr_en) begin
mem[mem_waddr] <= din;
end
end
assign full = (wp+plus_1 == rp) ? 1'b1 : 1'b0;
assign almost_full_node = (wp+plus_2 == rp) ? 1'b1 : 1'b0;
assign almost_full = full | almost_full_node;
// Read
always @(posedge clk) begin
if (rst) begin
mem_raddr <= 0;
rp <= 0;
end else begin
if (rd_en) begin
mem_raddr <= mem_raddr + 1;
rp <= rp + 1;
end
end
end
assign dout = mem[mem_raddr];
assign empty = (wp == rp) ? 1'b1 : 1'b0;
assign almost_empty_node = (wp == rp+plus_1) ? 1'b1 : 1'b0;
assign almost_empty = empty | almost_empty_node;
endmodule
`default_nettype wire
/* * mm_test.c * * Created on: 2016/06/29 * Author: ono */
#include <stdio.h>
#include <stdlib.h>
#include "xil_io.h"
#include "xparameters.h"
#include "sleep.h"
#include "xmotor_monitor.h"
int main(){
XMotor_monitor XMmoniL, XMmoniR;
XMotor_monitor_Config *XMmoniLPTR, *XMmoniRPTR;
XMmoniLPTR = XMotor_monitor_LookupConfig(0);
if(!XMmoniLPTR){
fprintf(stderr, "Left XMotor monitor configuration failed.\n");
return(-1);
}
int XMmL_status = XMotor_monitor_CfgInitialize(&XMmoniL, XMmoniLPTR);
if (XMmL_status != XST_SUCCESS){
fprintf(stderr, "Could not Initialize Left XMotor monitor\n");
return(-1);
}
XMmoniRPTR = XMotor_monitor_LookupConfig(1);
if(!XMmoniRPTR){
fprintf(stderr, "Right XMotor monitor configuration failed.\n");
return(-1);
}
int XMmR_status = XMotor_monitor_CfgInitialize(&XMmoniR, XMmoniRPTR);
if (XMmR_status != XST_SUCCESS){
fprintf(stderr, "Could not Initialize Right XMotor monitor\n");
return(-1);
}
while (!XMotor_monitor_IsIdle(&XMmoniL));
while (!XMotor_monitor_IsIdle(&XMmoniR));
XMotor_monitor_Start(&XMmoniL);
//XMotor_monitor_EnableAutoRestart(&XMmoniL);
while (!XMotor_monitor_IsIdle(&XMmoniL));
XMotor_monitor_Start(&XMmoniR);
//XMotor_monitor_EnableAutoRestart(&XMmoniR);
while (!XMotor_monitor_IsIdle(&XMmoniR));
u32 sa_countL = XMotor_monitor_Get_sa_count_V(&XMmoniL);
u32 sa_countR = XMotor_monitor_Get_sa_count_V(&XMmoniR);
u32 sb_levelL = XMotor_monitor_Get_sb_level_V(&XMmoniL);
u32 sb_levelR = XMotor_monitor_Get_sb_level_V(&XMmoniR);
u32 returnL = XMotor_monitor_Get_return(&XMmoniL);
u32 returnR = XMotor_monitor_Get_return(&XMmoniR);
printf("sa_countL = %d, sb_levelL = %d, returnL = %d\n", (unsigned int)sa_countL,
(unsigned int)sb_levelL, (unsigned int)returnL);
printf("sa_countR = %d, sb_levelR = %d, returnR = %d\n", (unsigned int)sa_countR,
(unsigned int)sb_levelR, (unsigned int)returnR);
return 0;
}
// motor_monitor.cpp
// 2016/06/15 by marsee
//
#include <ap_int.h>
#include <hls_stream.h>
#include "motor_monitor.h"
#define SUM_COUNT 100
// sa と sb の値を SUM_COUNT 分だけ積算して 1 か 0 かを決定する
void sum_sa_sb(hls::stream<ap_uint_1 >& sa, hls::stream<ap_uint_1 >& sb,
ap_uint_1 & sa_val, ap_uint_1 & sb_val){
ap_uint_1 sad;
ap_uint_1 sbd;
unsigned int sum_sa=0, sum_sb=0;
for (int i=0; i<SUM_COUNT; i++){
#pragma HLS PIPELINE II=1
sa >> sad;
sb >> sbd;
sum_sa += (unsigned int)sad;
sum_sb += (unsigned int)sbd;
}
if (sum_sa >= SUM_COUNT/2)
sa_val = 1;
else
sa_val = 0;
if (sum_sb >= SUM_COUNT/2)
sb_val = 1;
else
sb_val = 0;
}
int motor_monitor(hls::stream<ap_uint_1 >& sa, hls::stream<ap_uint_1 >& sb,
ap_uint<32> & sa_count, ap_uint_1 & sb_level, int & sum_count){
#pragma HLS INTERFACE ap_stable port=sum_count
#pragma HLS INTERFACE s_axilite port=sum_count
#pragma HLS INTERFACE s_axilite port=return
#pragma HLS INTERFACE s_axilite port=sb_level
#pragma HLS INTERFACE s_axilite port=sa_count
#pragma HLS INTERFACE ap_hs port=sa
#pragma HLS INTERFACE ap_hs port=sb
ap_uint_1 sad, sa_val, next_sa;
ap_uint_1 sbd, next_sb;
ap_uint<32> sac;
ap_uint_1 sble;
sum_count = SUM_COUNT+4; // sum_sa_sb() の呼び出しWaitを加算した
// 最初の値を保存する
sum_sa_sb(sa, sb, sa_val, next_sb);
for (sac=0; sac!=0xffffffff; sac++){ // saの値が変わらない間はループ
sum_sa_sb(sa, sb, next_sa, next_sb);
if (sa_val != next_sa)
break;
}
sa_val = next_sa;
if (sac == 0xffffffff) // overflow
return 1;
if (next_sa == 1) // next_sa が立ち上がり
sble = next_sb;
for (sac=1; sac!=0xffffffff; sac++){ // saの値が変わらない間はループ
sum_sa_sb(sa, sb, next_sa, next_sb);
if (sa_val != next_sa)
break;
}
sa_val = next_sa;
if (sac == 0xffffffff) // overflow
return 1;
if (next_sa == 1) // next_sa が立ち上がり
sble = next_sb;
for (sac++; sac!=0xffffffff; sac++){ // saの値が変わらない間はループ
sum_sa_sb(sa, sb, next_sa, next_sb);
if (sa_val != next_sa)
break;
}
if (sac == 0xffffffff) // overflow
return 1;
sa_count = sac;
sb_level = sble;
return 0;
}
// motor_monitor.h
// 2016/06/16 by marsee
//
#ifndef __MOTOR_MONITOR___
#define __MOTOR_MONITOR___
#include <ap_int.h>
typedef ap_uint<1> ap_uint_1;
#endif
// motor_monitor_tb.cpp
// 2016/06/15 by marsee
//
#include <ap_int.h>
#include <hls_stream.h>
#include "motor_monitor.h"
#define DATASIZE 2000
int motor_monitor(hls::stream<ap_uint_1 >& sa, hls::stream<ap_uint_1 >& sb,
ap_uint<32> & sa_count, ap_uint_1 & sb_level, int & sum_count);
int main(){
using namespace std;
hls::stream<ap_uint_1> sa;
hls::stream<ap_uint_1> sb;
ap_uint<32> sa_count;
ap_uint_1 sb_level;
int sum_count;
ap_uint_1 salv=0;
ap_uint_1 sblv=0;
int cnt = 0;
for (int j=0; j<(4*2); j++){
for (int i=0; i<DATASIZE/4; i++){
sa << salv;
sb << sblv;
}
if (cnt==0)
sblv = ~sblv;
else if (cnt==1)
salv = ~salv;
else if (cnt==2)
sblv = ~sblv;
else // cnt == 3
salv = ~salv;
if (cnt==3)
cnt = 0;
else
cnt++;
}
int ret_val = motor_monitor(sa, sb, sa_count, sb_level, sum_count);
printf("sa_count = %d, sb_level = %d, return value = %d, sum_count = %d\n",
(unsigned int)sa_count, (unsigned int)sb_level, ret_val, sum_count);
return 0;
}
日 | 月 | 火 | 水 | 木 | 金 | 土 |
---|---|---|---|---|---|---|
- | - | - | - | - | 1 | 2 |
3 | 4 | 5 | 6 | 7 | 8 | 9 |
10 | 11 | 12 | 13 | 14 | 15 | 16 |
17 | 18 | 19 | 20 | 21 | 22 | 23 |
24 | 25 | 26 | 27 | 28 | 29 | 30 |
31 | - | - | - | - | - | - |